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ABSTRACT 


The Naval Postgraduate School (NPS) is currently involved in a long-term 
project to investigate and develop real-time control software, artificial intelligence, 
computer architecture and control systems theory as they pertain to U.S. Navy 
autonomous vehicle programs. In support of this goal, the NPS is currently designing 
and fabricating a testbed autonomous underwater vehicle. 

This work describes the design, development, and testing of a Guidance 
Subsystem for this testbed vessel which utilizes portions of cubic spirals as the desired 
path to follow between waypoints. In addition, data translation firmware and real¬ 
time control software for the control surfaces and main motors is designed, 
implemented and tested. 

The process of selecting and implementing an appropriate computer 
architecture in support of these goals is also discussed and detailed, along with the 
choice of associated computer hardware and real-time operating system software. 
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I. INTRODUCTION 


In the next decade, Autonomous Underwater Vehicles (AUVs) will begin to 
perform missions which have previously been accomplished by attack submarines. 
AUVs could "...change the nature of undersea warfare..." and by the year 2025 they 
"...will be as important as manned systems and by 2050 will be the predominant 
[undersea] warfighting systems." [HOLZER 90] 

AUVs will perform such missions as underwater surveillance, tracking, 
minelaying, and possibly offensive antisubmarine warfare missions [BARKER 86]. 
Richard Rumpf, the assistant secretary for research, engineering and systems, in 
testimony to the House Armed Services Committee in April of 1990, emphasized the 
U.S, Navy’s commitment to AUVs "...for a variety of naval missions, including 
antisubmarine warfare." [HOLZER 90] 

Other prominent individuals see AUVs as "mission enhancers" for manned 
submarines, and feel that they will be "...integrated combat systems which are a part 
of a submarines sensor suite...." [BAKER 89] 

Current U.S. Navy schedules project the deployment of AUVs aboard the SSN- 
21 Seawolf by 2000, and on Improved SSN-688 Los Angeles-class submarines by 2005 
[ROBINSON 86, HOLZER 90]. 

Untethered, unmanned submersibles offer significant advantages over 
conventional manned submersible vessels: 
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• They can perform tasks which are considered too dangerous for humans to 
accomplish. 

• They can operate at tremendous depths, and can maneuver without regard for 
human physiological limitations. 

Along with these advantages come disadvantages, primarily the lack of human control 
and the inability to intervene in the event of unforeseen problems. To compensate 
for these shortfalls, a successful AUV must be able to incorporate the advantages of 
artificial intelligence, real-time control, environmental sensing and maneuverability 
into a compact, integrated package. 

The development of autonomous underwater vehicles which have these 
characteristics, and are capable of operating under a wide variety of situations and 
operating conditions, is an area of intense and diverse investigation. A prime 
problem in fielding an operational AUV is the requirement for "...robust software to 
maintain vehicle integrity while accomplishing mission goals...." [BELLINGHAM 89] 
At the Naval Postgraduate School (NPS), two AUVs have been constructed in an 
attempt to address some of this issues. 

The first NPS submersible, designated AUV 1, was 27 inches long and displaced 
less than 20 pounds, which allowed operation in a 40 foot long test tank. This vehicle 
had an umbilical to provide power to onboard control systems and had no onboard 
computer system. Vehicle control was provided by an off-hull PC AT microcomputer 
which transmitted plane commands through radio control equipment to the AUVs 
dive planes. [BRUNNER 55] 
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In order to allow completely autonomous behavior a second AUV (designated 
AUV II) is under construction. AUV II will support analysis of vehicle dynamics and 
control, real-time control systems theory, higher level artificial intelligent processing, 
robotics and computer architecture for future large AUVs. This research is being 
conducted as part of a long-term study funded and sponsored by the Naval Surface 
Weapons Center, White Oak, Maryland [HEALY 89b]. 

A. OBJECTIVES 

This thesis covers several tasks in support of the initial launch of AUV II 
including: 

(1) Selection of the Vehicle Control computer and data translation hardware and 
real-time computer operating systems which meet the requirements specified 
above 

(2) Design, development and testing of the vehicle Guidance subsystem 

(3) Development and testing of real-time control software for the control surfaces 
and main motors on AUV II. 

B. BACKGROUND 

1. Vehicle Characteristics 

The basic layout of AUV-2 is illustrated in Figure 1. The main vehicle 
body is constructed as a rectangular aluminum box with a beam width of 16 inches, 
a height of 10 inches, and a length overall (LOA) of 93 inches. It is anticipated that 
the finished vehicle will displace approximately 370 pounds. 
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Figure 1 - Basic Equipment Layout [GOOD 90] 


AUV-2 has independently controllable port and starboard bow planes, and 
port and starboard stern planes as well as independent fore and aft rudders. 

Propulsion is provided by four tunnel thrusters (fore and aft athwartships, 
fore and aft vertical) and two main screws aft. The tunnel thrusters, combined with 
the two main screws give the vehicle active control of five degrees of freedom (pitch, 
yaw, heave, sway and surge) at very low speeds (Hovering Mode). When the vehicle 
is operating at higher speeds (Transit Mode), all six degrees of freedom (pitch, roll, 
yaw, heave, sway, and surge) are actively controlled using the main screws for 
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propulsion and the hydrodynamic forces on the control surfaces to maintain desired 
pitch, roll and yaw rates. [KWAK 90] 


There is a sonar system consisting of four individual pencil-beam sonar 
transducers mounted in a flooded nose, an inertial sensor suite ( consisting of a flux 
gate compass, three rate gyros, three accelerometers, a vertical gyro and a directional 
gyro), a differential pressure sensing depth cell, a paddle wheel speed sensor, and 
individual motor RPM sensors. The overall combination of sensors and effectors 
results in a minimum requirement of 12 separate analog control output signals and 
23 different analog input signals for AUV-II (as summarized in Table I). 

There were numerous goals established for the development of AUV II. 
The vessel computer system had to be low cost, utilizing off-the shelf hardware where 
possible, be readily reconfigurable for different missions, and be expandable to multi¬ 
processor capability at a later date. The operating system and control software had 
to be easy to implement (preferably in the C language), had to use a commercially 
available operating system, must be able to simulate complex behavior, should 
support multi-tasking and should be easy to reconfigure for different missions. 

2. AUV Sk)ftware Classification 

Software for NPS AUV-II can be divided into two broad classes: Mission 
Planning software (off-vehicle non-real-time software) and Vehicle Control software 
(real-time on-vehicle software). 

Mission Planning is the process of determining a reference path to be 
followed by the AUV based on knowledge of pre-existent obstacles, threats and other 
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Table I - Analog Input and Output Signals of AUV II 


ANALOG INPUT 
SIGNALS 


ANALOG OUTPUT 
SIGNALS 


Port and Starboard Bow planes (2) 


Port and Starboard Stem planes (2) 


Fore and aft Rudders (2) 


Port and Starboard main engines (2) 


Fore and aft horizontal thrusters (2) 


Fore and aft vertical thrusters (2) 



Control Surface Position (5) 


Propulsor RPM (6) 


Pitch/Pitch Rate (2) 


Roll/Roll Rate (2) 


YawA^aw Rate (2) 


Depth (1) 


Sonar Channels (4) 


Speed (1) 


Health and Well-being (4) 


possible hazards to the vehicle. The Mission Planner constructs a path as a series of 
waypoints for the vessel to reach. Development of the Mission Planner for AUV II 
is described in [ONG 90], This obstacle-free path is downloaded to the vehicle via 
a detachable RS-232 serial link. 

The Vehicle Control software utilizes the path information from the 
Mission Planner and controls the vehicle from point of origin to mission goal. As the 
mission progresses, the Vehicle Control system analyzes all input signals from 
onboard sensors and provides output signals to control surfaces, thrusters and main 


motors while piloting the vessel to the desired goal. 
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The Control System Software Architecture for AUV II is illustrated in 


Figure 2 and the proposed hardware architecture is shown in Figure 3. 



Figure 2 - Control Systems Software Architecture 


An off-huli LISP machine (currently a Texas Instruments Micro-Explorer) 
generates and downloads the Mission Control software to AUV-2 just prior to 
deployment. The current Mission Control software consists of a series of waypoints 
to be achieved during the mission. A waypoint is a quadruple made up of the desiied 
three-dimensional position (x,y,z) combined with desired vehicle velocity (v). The 
quantity of information available in a waypoint is an important consideration in the 
development of the Local Path-Planner (LPP) and is discussed fully in Chapter 3. 

3. Real Time Vehicle Control Requirements 

AUV II requires a complex real-time control system which must be able 


to simultaneously handle a myriad of tasks. These tasks range from the lowest level, 
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Figure 3 - Proposed Hardware Configuration 


such as managing interrupts and processing the sensor input/output specified above, 
to very high level tasks, such as obstacle avoidance and path-replanning. The 
development of this type of system is dependent on numerous constraints which are 
generated by the nature of the problem such as: 

• program and mission complexity 

• data processing and storage requirements 

• control loop timing 

as well as constraints generated by the choice of hardware: 

• processor speed 

• data translation device speed 

• communication bandwidth 

• compiler and debugger capability 
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Tests with NPS AUV-I demonstrated that an IBM AT-clone could provide 
autonomous control for diving plane operation while controlling a single propulsor. 
With AUV-II, both the number of controls required and their complexity will be 
increased. 

For AUV II the Autopilot control loop must be closed at a minimum of 
10 times per second (every 100 msec). The Vehicle Control computer system must 
be able to multi-task, and should be expandable to allow additional of data 
translation, memory and processor capability [HEALY 90]. 

4. Guidance Subsystem Requirements 

The Guidance subsystem receives waypoints (x,y,z,v) from the Mission 
Planner. These waypoints correspond to points on a three-dimensional grid being 
traversed by the vehicle. These waypoints must be fitted to a smooth path, and the 
path information must be fed to the Autopilot subsystem as series of intermediate 
positions (x,y,z,v) with heading and pitch angle (0,<^>). These intermediate positions 
must be fed at a rate which will allow smooth operation of the AUV. 

There have been numerous methods of trajectory planning developed in 
the past decade [BRADY 82, KHATIB 85, SHIN 86]. These methods were all based 
on a set path in cartesian space which could be transformed to a set of points which 
describe the motion through inverse kinematic equations. It has been shown that 
jerk, the third derivative of position of the desired trajectory, adversely affects control 
algorithms and therefore should be minimized [KYRIAKOPOLIJS 55]. It is also 
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intuitive that in an underwater vessel, jerk can not be tolerated because of the 
hydrodynamic constraints in place on the vehicle. 

Kanayama and Hartman originally proposed the utilization of the 
derivative of path curvature of cubic spirals* as a cost function for path smoothness 
in a local path planner for an autonomous vehicle [KANAYAMA 55a]. The cost 
function utilized in the preparation of the path between postures is the integral of the 
square of the derivative of curvature of the path which minimizes jerk. 

Kanayama’s method of guidance was first implemented on Yamabico-11, 
an autonomous land-based mobile robot [KANAYAMA 556]. The elegance and 
simplicity of Kanayama’s method was attractive, and appeared to be readily adaptable 
to AUV control as well. 

Kanayama also developed a set of equations providing stable feed-forward 
tracking control for Yamabico 11 [KANAYAMA 90\. His method utilized postures 
(x,y,0) as input from the 'Mission Planning’ level, and output postures and velocities 
to the Autopilot. In this thesis, both Kanayama’s guidance and tracking control 
methods have been modified for implementation and use on NPS AUV II. 

C. THESIS ORGANIZATION 

Chapter II presents a survey of previous work on AUV systems and associated 
technology. Current operational AUV and Unmanned Underwater Vehicle (UUV) 


* A cubic spiral is a curve whose tangent direction is described by a cubic function of 
length. 
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systems are described, and their effect on the hardware and software decisions made 
for AUV II is presented. 

Chapter III provides a detailed description of the vehicle computer hardware 
and associated low level software. This includes a review of the rationale for the 
choices made as well as a discussion of the development of the control firmware 
(data conversion and control) routines which were created for AUV II. 

Chapter IV discussed the development and implementation of the Guidance 
Subsystem including the on-line and off-line local path planners, and the tracking 
controller. Background information necessary to understand the Guidance problem 
for AUV II, and the relationship between Guidance and applicable software modules, 
is also provided. 

Chapter V provides a summarization of the overall work, and gives 
recommendations for possible extension, expansion and improvement. This chapter 
also discusses a review of the contributions made to autonomous vehicle 
development. 
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II. RELATED WORK 


This chapter provides an overview of current work in the field of autonomous 
vehicle control as well as related topics pertinent to the design of the AUVII Vehicle 
Control Computer and the Guidance subsystem are also presented. The effect of 
these other projects on the hardware and software decisions made for AUV II is 
summarized. 

A. EDUCATIONAL INSTITUTIONS 

1. Texas A&M 

Texas A«S:M UnVersity has proposed the use of a modified blackboard 
system based on "situations" as the structural design paradigm for the knowledge 
based control architecture in AUVs. These situations are defined as "...the rule sets, 
domain and declarative knowledge required to make the decisions, judgements and 
actions required of the reasoning component in the corresponding part of the 
problem space...." [MAYER 87] A prototype implementation of this strategy has been 
in incorporated on four Symbolics 3640 machines, but the system does not appear to 
have been implemented on an operational vessel. 

2. University of Florida 

The University of Florida has done work on a hierarchical control system 
for autonomous vehicles based on work by Sardis. The core of this system is a three- 
level "Intelligent Module" controller consisting of a Planner, Navigator and Pilot 
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which all interface with a Cartographer, and a separate Low-Level Controller 
{ISIK 84\. This system has been modeled, but reference to an operational vessel has 
not been found. 

3. Georgia Institute of Technolc^ 

Arkin at Georgia Institute of Technology proposes the use of "Motor 
Schema" as the basic unit of behavior for navigation [ARKIN 87]. These "schema" 
are multiple concurrent processes which operate in parallel with sensory schema to 
provide vehicle motion. This differs from Brooks’ subsumptive architecture 
[BROOKS 55] in that it avoids layering entirely and instead uses a "souplike" network 
of schemas which can change dynamically based on the vehicles current needs, 
perceptions and goals [ARKIN 87]. 

4. University of New Hampshire (UNH) 

UNH has been involved in submersible technology since 1977 and has 
constructed and launched three generations of autonomous vehicles. The current 
generation of AUVs are the HAVE III vehicles. The HAVE III has a modular, 
hierarchical computer architecture and utilizes three Motorola 68000 based 
computers on a VME bus running under the pSOS operating system [lALBERT 55]. 
The lowest level software is coded in ’C and the upper level is coded in 'LISP*. 
The vehicle is designed with a blackboard based system for context sensitive higher 
level mission planning [CHAPPELL 57], but these vehicles have also successfully 
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operated under the NIST NASRAM architecture (MUST vehicles) described in the 
NIST section below. 

5. MIT - Sea Grant College 

The Massachusetts Institute of Technology - Sea Grant College (MIT - Sea 
Grant) has built a small AUV as a software testbed. The vehicle displaces 
approximately 60 pounds, utilizes three thrusters, has a depth rating of 200 feet and 
endurance of approximately 2 hours [BELLINGHAM 90]. The initial mission 
selected for the vehicle is to avoid obstacles while proceeding to a stationary target. 
In pursuit of this goal, the vessel must utilize several "intelligent behaviors" including: 
homing, obstacle avoidance and information gathering [BELLINGHAM 89]. The 
vehicle utilizes a GESPAC-MPU20 68020 based processor running under the OS*9 
operating system. Software is writte.i in C on an IBM AT compatible and cross- 
complied for the 68020. Prior to the start of a mission, software is downloaded to the 
AUV and activated through a serial link tether. When the mission is complete, the 
tether is reattached and data is uploaded to the IBM AT clone for analysis. 

The MIT - Sea Grant vessel utilizes a "Layered Control" system adapted 
from work done by Brooks with land-based robots [BROOKS 88, 89]. This 
architecture is modular in that a mission is broken into discrete behaviors which can 
output commands. It is reflexive since behaviors respond to immediate sensor 
readings, thus world models are not required. Behaviors are hierarchically prioritized, 
and completely asynchronous [BELLINGHAM 90]. These modular behaviors are 
combined to produce intelligent behavior. 
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6. University of California - Santa Barbara 

Kanayama developed a Guidance system for a mobile land robot called 
Yamabico-11 which utilizes a set of postures (similar to AUV II waypoints) for local 
path control [KANAYAMA 89a, 89b]. This system utilizes portions of cubic spirals 
as the path to be followed between postures. The cubic spiral has the property of 
minimizing jerk (the second derivative of acceleration) between postures which is very 
desirable for an underwater vehicle. This system operated in real-time and is 
implemented in the C language. 

B. INDUSTRY AND GOVERNMENT 
1. FMC Corporation 

FMC Corporation (FMC) has developed a hierarchical real-time control 
system architecture where there is an "order of ten" space-time difference between 
hierarchical levels [NITAO 55]. The upper levels in this hierarchy possess a broad, 
abstract view of the world. Since actions in these higher levels do not affect the 
narrow, detailed real world of the lower levels, the processing times can be 
significantly slower than at lower levels. 

This method also employs a Reflexive Pilot which does not utilize a 
memory map of past subgoals. Higher level replanning is only required when the 
Reflexive Pilot fails to make progress [NITAO 86]. 

FMC is also developing a control system which does path-planning based 
on 3-D digital maps. They assert that the system can be incorporated into any multi- 
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level functional architecture. This path planning strategy utilizes a three-tiered system 
where the User (or Action Planner) furnishes input to a Global Path Planner, which 
then computes a path to provide to the Local Path Planner for execution 
[PARODI84]. 

2. Rockwell 

Rockwell is working on an Expert System to "dynamically schedule the 
allocation of resources" onboard satellites. This system incorporates several 
"intelligent agents" such as a Satellite Controller, a Planner, and a Subsystem 
Specialist [BARRY 55]. This system is functionally very similar to the FMC system 
and to the system being developed for NPS AUV II. The Satellite Controller 
generates an "agenda" (mission) which is provided to the Planner. The Planner 
generates a plan (path) for the Subsystem Specialist (Pilot) to carry out. 

3. Martin-Marietta 

Martin-Marietta Aero and Naval System’s Mobile Undersea Systems Test 
(MUST) Laboratory has been working since 1984 to develop a testbed for AUV 
technology development. They have constructed a 30 foot long vessel which is 
capable of diving to 2000 feet. The MUST vehicle is controlled by two Motorola 
68020 processors on a VME buss utilizing a commercial operating system. All I/O 
and memory cards are also off-the-shelf circuit boards. Research payloads will drive 
the control system at a 10 Hz rate. The vessel uses a "...modular, event-driven state 
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table process system..." designed around the NIST NASREM Real-Time Control 
System (RCS) [HEBERT 87]. 

4. National Institute of Standards and Technology 

The National Institute of Standards and Technology (NIST) proposes the 
use of the NASA/NBS Standard Reference Model for Telerobot Control System 
Architecture (NASREM) a hierarchical control system developed by Albus 
[ALBUS 89\. This architecture incorporates several layers of control, each of which 
provides commands to the next lower layer. Desired vehicle actions are input at the 
highest level and decomposed both spatially and temporally [ALBUS 55]. As 
commands are passed to lower levels they are further decomposed (both temporally 
and spatially) until they become specialized enough for vehicle control. 

Each layer has its own sensory processing, world modeling and task 
decomposition which allow it to sense the environment and take appropriate action. 
The system supports up to seven or more layers, all of which can share common 
memory as a blackboard, for map storage, object files, etc. 

The command timing structure in the NIST architecture is similar to that 
used by UNH. Commands are passed at a time interval of approximately 10 times 
the time interval of the next lower layer [ALBUS 90]. 

The NIST NASREM system architecture was tested on two UNH EAVE 
III vehicles in a Multiple Autonomous Vehicle (MAUV) experiment funded by the 
DARPA Naval Technology Office [ALBUS 55, HERMAN 55]. This experiment 
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utilized the hierarchical structure of NASREM in conjunction with UNH’s lower level 
control code. 

5. Woods Hole Oceanographic Institute 

Woods Hole has developed a very capable double tethered vehicle system 
(ARGO-JASON) \YOEGER 90]. This system utilizes 8086 based processors running 
under the VERTEX operating system. The ARGO-JASON system has the ability 
to operate in an Auto-Heading or Auto-Depth mode, but does not operate with total 
autonomy. 

6. NOSC 

The Naval Ocean Systems Center (NOSC) has configured the EAVE-West 
vehicle to demonstrate distributable software architecture for AUV plan execution 
[DURHAM 87\. NOSC is also doing work with the possible application of neural 
network technology to autonomous vehicles [DURHAM 90]. 

7. International Submarine Engineering 

International Submarine Engineering Research, Ltd. (ISE) has developed 
several untethered, remotely controlled submersibles. The first autonomous 
remotely controlled submersible (ARCS) was designed as a hydrodynamic survey 
vehicle which follow a series of waypoints. ARCS had a control system consisting of 
three Intel microprocessors on a common bus utilizing common memory 
[THOMAS 85, JACKSON 84]. 
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DOLPHIN is a diesel powered UUV designed by ISE. It was originally 
developed as an offshore hydrographic mapping vessel, but has since been revamped 
for use as a search and survey vehicle, advanced hydrographic survey and seismic 
exploration. The vehicle control computer on DOLPHIN uses the GESPAC G-96 
bus with Motorola 68010 and 680008 processors. ISE conducted an extensive market 
survey, and chose the GESPAC system based on price, performance, size ruggedness 
and availability [WIETZEL 87]. 

Because of the increased complexity of the real-time control systems on 
autonomous submersibles, ISE is pursuing the development of a complex object- 
oriented real-time control system. This system bases system operation on events and 
actions and utilizes a simple, efficient preemptive scheduler [ZHENG 86]. This 
scheduler is extremely modular, and can be adapted to a variety of different hardware 
architectures, and has been tested on Intel based PCs and Motorola 68000/68010 
based GESPAC systems. 

C. SUMMARY 

There are very few operational underwater vehicles which are truly autonomous. 
A summary of the capabilities of vessels which are truly autonomous is given in 
Table II. Of the AUVs listed, the MIT Sea Sprite most nearly approximates AUV II 
in size and intended use. 
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Table II - Current AUV capability [after BELLINGHAM 90] 


VEHICLE 

Thrust 

Comp 

Sens 

OA 

ARCS (ISE) 

If 

3(68010) 

D 

yes 

EAVE III (UNH) 

2f,2v,21 

nail 

D,T 

yes 

Sea Squirt (MIT) 

2f,21 

1(68020) 

D,T 

dev 

MUST (Martin-Marietta) 

lf,2v 

2(68020) 

D 

dev? 

EAVE-West (NOSC) 

2f,lv 

1(8080) 

D,P,T 

no 


Thruster Type : f=forward, v=vertical, l=lateral 
Computer Type : qty(processor type) 

Sensors: D=depth, T=temperature, P=photography 

Obst. Avoid : yes, no, dev=under development 

The following observations were based on known AUV systems, and were central to 
the choices made for the AUV II Vehicle Control Computer system: 

• There does not appear to be a clear standard for real-time operating systems 
for AUVs. 

• The 'C language appears to be the preferred language for control-level 
coding. 

• Several AUVs utilize the GESPAC Motorola-based architecture. 

• Most vehicles have a modular, hierarchical control structure, with each level 
operating about ten times faster than the layer which lies lower in the heirarchy. 

• The cubic spiral system Kanayama created for Yamabico-11 is readily adaptable 
for use in AUV II. 
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III. EVOLUTION OF ONBOARD COMPUTER SYSTEM 

This chapter presents the evolution of the Vehicle Control Computer system 
for AUV II. The Vehicle Control Computer system consists of a GRiDCASE 1535 
EXP based on a 12.5 MHz Intel 80386 processor and a GESPAC system centered 
around a 25 MHz Motorola 68030 processor. 

A. GRiDCASE 1535 EXP 

A survey of available systems which could meet the increased computational 
requirements for AUV-II conducted prior to the start of this thesis resulted in the 
purchase of the GRiDCASE 1535 EXP (GRiDCASE). This is a 12.5 Mhz 80386- 
based laptop computer which runs under a version of MS-DOS modified by the 
GRiD corporation. The purchased configuration included an 80387 math 
coprocessor, 4 Mbyte of RAM, a 40 MByte hard drive, a 2400 baud internal modem, 
and an expansion tray which allowed for the addition of one AT-compatible 
expansion card and one XT compatible expansion card [GRID 88]. 

1. Data Translation Boards 

Two Data Translation (DT) cards were purchased to provide analog-to- 
digital and digital-to-analog capability for the GRiDCASE. The first was a DT2821 
High-Speed DMA combined analog-to-digital/digital-to-analog converter and the 
second was a DT2815 digital-to-analog converter [DATA 89]. The DT2821 provided 
16 channels of digital input/output, 2 channel of 12-bit digital-to-analog outputs and 
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16 single-ended (or 8 differential) input analog-to-digital inputs. The DT2815 
provided 8 channels of 12-bit digital-to-analog output. The combined total capability 
of the system (which used both available card slots on the GRIDCASE) was; 

• 10 channels digital-to-analog output 

• 8 channels analog-to-digital input 

• 16 channels digital input 

As can be seen from Table I this combination did not meet the initial specified data 
translation requirements, but it was thought that satisfactoiy operation could be 
achieved if some input signals were multiplexed and if control surfaces (such as bow 
planes and stem planes) were ganged together. (In fact, multiplexing would be 
undesirable since demultiplexing of the input signals would require additional CPU 
time during execution of the primary control loop.) 

Test programs were written to verify proper operation of the DT boards. 
Simple input/output tests were successful, but attempts to utilize the DMA capability 
of the DT2821 failed repeatedly. Several versions of code were written in an attempt 
to get proper DMA operation. Numerous calls to technical support personnel at 
both Data Translation and GRiD over a period of months were fruitless. Each 
vendor attempted to point to the other as being the source of the problem. The final 
result was that DMA did not function properly. 

2. Real-Time Operating System 

Because of the timing requirements for the Vehicle control computer 
(control loop must be executed at a minimum 10 Hz rate) it was decided that the 
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new system should be able to multi-task (at least to the point of allowing the 
Autopilot to operate independently of all other modules). This posed an interesting 
problem, in that the GRiDCASE computer was operating under MS-DOS, which 
does not readily support multi-tasking. 

a. TSR Program 

A simple evaluation program was written to determine if a single 
control program could operate at a specific time interval (see Appendix A). The 
program written was a terminate-and-stay-resident (TSR) program that operated at 
a 100 msec frequency, while allowing another program to operate in the foreground 
at all other times [DETTMAN 55]. This program took advantage of DOS interrupt 
28h (the timer interrupt) and showed that two programs could be operated 
successfully under MS-DOS. 

The primary disadvantage to the TSR method was that all procedures 
and code which were included in the TSR loop had to be completely devoid of DOS 
level calls (because the TSR interrupt INT 28h operates below the DOS level). If 
this method were chosen for Full Scale Development, all C function calls would have 
to be evaluated to ensure proper operation and many would have to be rewritten. 
As an example, the C printf would not work because it utilizes DOS calls when 
printing to screen, and so would have to be modified to use BIOS level calls for 
screen input/output. 
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h. Regulus Operating System 

In order to provide a true multi-tasking capability, it was decided to 
purchase and evaluate the Regulus operating system. Regulus is a UNIX-like 
operating system that provides such features as real-time tasks, shared memory data 
segments and user access to physical memory \ALCYON 86]. Regulus was purchased 
and installed on a GRIDCASE EXP-1535. The installation procedures for Regulus 
assumed that the host machine was an 80386 machine operating under true MS-DOS. 
Because the GRiDCASE operates under a modified version of MS-DOS there were 
some minor problems with installation. The Regulus operating system is relatively 
new, and the user manuals offer little insight into the systems multi-tasking 
capabilities. Regulus’ customer support system was able to offer little help on 
applications, and gave the impression that system performance would be significantly 
degraded in a multi-tasking mode using high-speed DMA. 

3. Hardware Problems 

There were now several distinct problems with the original hardware and 
software purchased for AUV-II: 

• There was no room for expansion (both card slots were filled). 

• There were inadequate A2D and D2A channels for required input and output 
signals. 

• A maximum of two tasks could operate under DOS "simultaneously" (one a 
TSR), and the TSR task must be entirely devoid of DOS system level calls. 

• The chosen machine was relatively slow (12.5 Mhz). 
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• Since the converter boards were from a different manufacturer (DT) than the 
computer (GRiD), compatibility problems were difficult to resolve. 

• The multi-tasking operating system (Regulus) might eventually work, but once 
again there were incompatibilities due to the modified MS-DOS operating 
system used by GRiD. 

Because of these problems, a new survey for a real-time control computer 
system and operating system was conducted. A true multi-tasking operating system 
was desired. In addition, the new system must be expandable to multi-processor 
capability and have the ability to add additional data conversion capability. A final 
requirement was that other autonomous vehicle projects be using the system, to 
ensure that the system would work in an undersea environment, and to allow transfer 
of information and low-level source code. 

B. GESPAC 

After the survey of literature discussed in Chapter 2 was complete, several 
organizations were contacted directly about appropriate hardware. Discussions with 
the Monterey Bay Aquarium Research Institute (MBARI), International Submarine 
Explorations (ISE), Naval Ocean System Center (NOSC), the University of New 
Hampshire (UNH) and the Sea Grant College at Massachusetts Institute of 
Technology (MIT - Sea Grant) resulted in the choice of GESPAC. 

GESPAC offers a variety of Motorola 68000 based systems. The GESPAC 
systems operate under the OS-9 commercial operating system produced by 
Microware [GESPAC 88]. OS-9 is a true multi-tasking operating system 
[MJCROWARE 87] and is currently in use in the MIT - Sea Sprite AUV, (allowing 
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possible use of MIT Sea Sprite data conversion code). GESPAC manufactures their 
own data conversion cards which minimizes the possibility problem of hardware 
incompatibility [GESPAC 89]. Both ISE and MIT have successfully utilized GESPAC 
systems on autonomous vehicles. It was decided to purchase two GESPAC systems 
with similar data translation capabilities for AUV II. 

1. Target System 

The system to be placed on AUV II is designated the target system . It 
utilizes a GESPAC MPU30HF main processor board with a Motorola 68030 CPU 
and 68882 FPU operating at 25 MHz. This board is configured with 2.5 Mb of on¬ 
board RAM and has room for 4 Mb of EPROMS. To meet the data translation 
requirements specified in Chapter 1, four additional boards were required: an ADAl 
(16 SE/8 DE analog-to-digital input channels, 4 digital-to-analog output channels); an 
ADC2B (16 SE/8 DE analog-to-digital input); a DAC2B (8 digital-to-analog output 
channels); and a MFI card (2 serial ports, 2 8-bit parallel ports). A GESPAC SCSI 
hard disk controller card and a 200 MB Conner peripheral hard drive will be utilized 
for data storage. The six GESPAC cards are placed in a 12 card rack to be mounted 
in AUV II, and not only met all known data processing requirements, but also 
allowed six additional slots for future expansion. 

For the target system a PC Bridge cross-compiler and source level 
debugger was purchased. PC Bridge allows an IBM compatible machine to serve as 
a terminal and data storage device for the development system, which currently has 
no disk storage. With this system, control software for the vehicle can be written in 


26 







C on an IBM PC compatible machine, compiled and debugged using PC Bridge, and 
then downloaded via an RS-232 serial link to the vehicle for testing. When the 
vehicle is ready for launch, mission software can be downloaded to the vessel using 
PC Bridge and a serial link. 

2. Development System 

The off-hull OS-9 system has been designated the development system . 
It utilizes identical processor and data translation boards as the target system. This 
allows for any of the boards in this system to be transferred to the target system in 
the event of a board failure. 

In addition, this system is configured with a 40 MByte hard-drive and the 
OS-9 development compiler and debugger. This system operated entirely under OS-9 
(PC Bridge operates under MS-DOS) and therefore cross-compilation is not required. 

A second advantage of the development system is that when the low level 
control code has been adequately debugged, this system is ideally suited to prepare 
Motorola S-records which can be burned into EPROMs and placed on the 
MPU30HF processor board. This will free up RAM, and speed up the downloading 
of mission control code. 
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C. AUV II DISTRIBUTED COMPUTER SYSTEM 


The AUV II has a distributed computer control system divided into three major 
components: 

(1) Vehicle Control Computer (VCC) - onboard computer which provides active 
vehicle control at the lowest level. It passes information to/from the Mission 
Replanner. 

(2) Mission Replanner - onboard computer which performs recalculation of 
mission plan as required. Provides mission plan to VCC as a series of waypoints. 
Receives vehicle feedback and sensor data necessary for mission replanning from 
the VCC. 

(3) Mission Planner - off-hull computer which constructs mission based on global 
world model and data. This is the only computer with direct operator interaction. 
Downloads mission to Mission Replanner. 

The current configuration utilizes the GESPAC as the Vehicle Control Computer and 

a Texas Instruments MicroExplorer as the Mission Planner. There is no software for 

the Mission Replanner, so the mission plan is downloaded directly into the Vehicle 

Control Computer via an RS-232 serial connection. 

The final configuration of the on-board computer system will utilize both the 
GRiDCASE and GESPAC systems. The GESPAC will continue to be the Vehicle 
Control Computer, performing all actual real-time vehicle control and data 
translation functions. The GRiDCASE will be used as the Mission Replanner, 
receiving the Mission Plan as a series of waypoints via the RS-232 serial link from the 
off-hull Mission Planner and providing them to the Vehicle Control Computer as 
required. In addition, if obstacles are encountered while traversing from waypoint 
to waypoint, the Mission Replanner will perform path planning functions similar to 
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the Mission Planner to create a modified Mission Plan in real-time without operator 
interface. 


D. DATA TRANSLATION SOFTWARE 

The GESPAC data translation boards are provided without associated data 
translation software. In order to properly test the vehicle control software and 
hardware, it was necessary to write the necessary data conversion routines. The code 
for these data translation and conversion routines is provided as Appendix B. 

1. Digital-to-Analog Routines 

Each GESPAC digital-to-analog board is assigned a specific hardware 
address on the G-96 bus. Individual devices are assigned unique channels on a 
specific board. As an example, the STBD_MAIN motor is assigned to channel 0 of 
the DAC2B board. 

The user routine accesses the desired control device by a call to a function 
specifying a individual board type with a channel number and voltage to be written 
to that channel (e.g. DAC2B(STBD_MAIN,1000)). 

2. Analog-to-Digital Routines 

The analog-to-digital routines work in a similar manner. The user routine 
specifies a specific board and channel to be read (e.g. ADA1(STBD_MAIN_RPM)). 
The analog-to-digital routine reads the specified channel and returns an integer value 
corresponding to the analog value read. 
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E. CONTROL OF HYDRODYNAMIC SURFACES 


The hydrodynamic control surfaces on AUV II are being driven by Airtronics 
94510 model airplane control servos. This type of servo requires a pulse-width 
modulated control voltage to properly position the device. The servos are powered 
by an eight-channel pulse-width modulator (PWM) designed by an NFS staff 
electronics technician. The schematic diagram for this device is provided in Figure 4. 



Figure 4 - Pulse-Width Modulator Schematic 


The PWM receives a zero to ten volt control signal from the digital-to-analog 
boards on the GESPAC control computer system and outputs a 0.5 to 1.5 millisecond 
pulse to the control surface servo motors. The pulse width is a nominal 1 millisecond 
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with a 5 volt input signal. The output is exponentially non-linear but highly 
repeatable. 

Output pulses are triggered by negative going synchronizing oscillator input 
signals. This signal releases an internal shorting transistor across the output timing 
capacitor. The output capacitor charges exponentially through the timing resistor 
until the output voltage reaches the value of the control input voltage, resulting in an 
exponential relationship between input voltage and output pulse width. This pulse- 
width modulated voltage is provided to the appropriate control surface servo motor. 

1. Servo Control Testing 

Tests were conducted using two different servos and all eight channels of 
the PWM. The servo test setup is shown in Figure 5. A variable-voltage DC power 
supply was connected to the PWM control line of each channel. The power supply 
voltage was varied to achieve control surface positions from -45 to +45 degrees in 
15 degree increments. The voltages were checked in both increasing and decreasing 
directions to verify that the direction of approach did not matter. 

The data recorded was extremely stable. The power supply voltage varied 
less than 20 mV for all combinations of servos and channels. The servo data 
obtained is provided in Table III and is graphed in Figure 6. 

It was expected that the PWM output would be exponentially non-linear 
because the output pulse-width is determined by the charging of the capacitor 
connected to TSH of the NE455 (see Figure 4). Experimental results confirmed this 
hypothesis and the exponentially non-linearity can be seen in Figure 6. 
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2. Servo Control Signal Calculation 

The program CALC_SVO.C (see Appendix C) was written to create a 
look-up table of digital-to-analog signals which provide linear control surface response 
over the range from -45 to +45 degrees in one degree increments. The program 
does linear extrapolation of the data contained in Table III and writes to a look-up 
table file called "servo.dat". 

The linear interpolation is based on the equation: 
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where is the desired voltage, is the voltage at the next data point, V^p is the 
voltage at the last data point, is the desired angle, y4ngpp is the angle at the last 
data point, and Inc is the absolute difference between data points (in degrees). 

3. Manipulation of Control Surfaces 

The Autopilot manipulates the control surfaces through the function 
send_servo which takes two arguments: 

• SURFACE - the desired control surface to reposition 

• ANGLE - the desired position of that surface 

send_servo does a table look-up of the appropriate digital-to-analog signal in 
"servo.dat" based on the desired position input. This linearized value is passed to the 
control surface digital-to-analog channel, which then sends the proper control voltage 
to the PWM. The control signal is pulse-width modulated and sent to the control 
surface which is positioned to the desired angle. 

The calculated table values were compared with actual position and the 
resultant deviation was less than one degree from desired position over the entire 
control surface input range (-45 to +45 degrees). 

F. MAIN MOTOR CONTROL 

The two main propellers on the submersible are driven by Pittman 14202 
(WDG #3) 24 Volt DC servo motors [PUTMAN 87]. These motors are fed from a 
motor controller designed and fabricated at NPS by a staff electronics technician. 
The schematic diagram of the controller is provided as Figure 7. 
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Figure 7 > Motor Controller Schematic 


The circuit is a voltage amplifier with a bridge output. Two identical operational 
amplifiers channels are connected 180 degrees out of phase to Darlington output 
power transistor pairs which drive the motor. 

The motor controller receives zero to ten volt control signals from the 
G ESP AC digital-to-analog converters. A zero volt control signal results in maximum 
negative voltage output (corresponding to maximum astern RPM) and a ten volt 
control signal results in maximum positive output being applied to the main motors 
(corresponding to maximum ahead RPM). A five volt control signal results zero 
voltage output and the motor remains stationary. 
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The current Autopilot outputs RPM commands to the main motors (vice 
outputting velocity commands) {RILING 90]. To achieve these RPM commands, a 
conversion routine was created which converts desired RPM values into appropriate 
voltage counts for the digital-to-analog boards, which then provide control voltages 
to the motor controllers to achieve the desired RPM. 

1. Main Motor Control Testing 

A system test tank was set up to calibrate the main motor controller. The 
motor test unit was designed to closely approximate the expected vehicle operating 
conditions and included a motor, propeller and shaft, and Kort nozzle as shown in 
Figure 8. 

A DC power supply was connected to provide motor drive power in the 
voltage range expected during vehicle operation (20 to 30 VDC) and a variable DC 
power supply was connected to the control voltage input of the controller. A strobo- 
tach was used to monitor actual shaft RPM. 

The variable DC power supply was adjusted to achieve motor speeds from 
zero to 700 RPM in both the ahead and astern directions. 700 RPM was chosen as 
the maximum value because motor current was approaching the five ampere limit of 
the test equipment circuitry. The test was conducted with motor drive supply 
voltages of 20 VDC, 24 VDC and 30 VDC. 

The control voltage required to achieve a desired motor RPM value was 
surprisingly independent of motor supply voltage. The control signal required 
deviated less than 20 mV as the main motor supply was varied from 20 to 30 volts. 
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Figure 8 - Main Motor Test Setup 

The average values obtained from these tests are recorded in Table IV and are 
graphically shown in Figure 9, 

2. Main Motor Control Signal Calculation 

Since the control voltage to RPM relation is non-linear, a software look-up 
table was needed to linearize the relationship. 

CALC_MN.C (Appendix D) creates a lookup table of digital-to-analog 
signals which provides linearized response in ten RPM increments from zero to 700 
RPM in both the ahead and astern directions. This program does linear interpolation 
of the data contained in Table IV and writes to a look-up table contained in the file 
"main mtr.dat". 
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Figure 9 - Ra' • Main Motor Voltages 
Table FV - Main Motor Data 


RPM 

Voltage (Ahead) 

Voltage (Astern) 

100 

5.319 

4.670 

200 

5.558 

4.450 

300 

5.902 

4.140 

400 

6.387 

3.739 

500 

6.898 

3.250 

600 

7.498 

2.730 

700 

8.280 

2.010 
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The linear interpolation is based on the equation; 


* y„ 

where is the desired voltage, is the voltage at the next data point, is the 
voltage at the last data point, RPM^^ is the desired RPM, is the RPM at the 

last data point, and Inc is the absolute difference between data points (in RPM). 

3. Control of Main Motors 

The Autopilot controls main motor speed through a call to MAIN_RPM 
which takes two arguments: 

• MOTOR - the desired main motor (Port or Stbd) 

• RPM - the desired motor RPM (-700 to +700) 

MAIN_RPM normalizes the requested RPM (to 0-1400 RPM) and does a table look¬ 
up of the appropriate digital-to-analog signal in "main_mtr.dat" based on the desired 
motor RPM. This linearized value is passed to the appropriate main motor digital-to- 
analog channel, which then sends the proper control voltage to the motor controller. 
The control signal is amplified and sent to the main motor. 

The calculated table values for the main motors were checked against 
actual motor RPM at supply voltages from 20 to 30 VDC over the main motor input 
range (700 RPM astern to 700 RPM ahead) with an observed deviation of less than 
one percent. 
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IV. GUIDANCE SUBSYSTEM 

The purpose of the Guidance subsystem is to provide desired waypoint, heading 
and velocity input to the Autopilot subsystem in a manner which will minimiz e jerk. 
In order to achieve this goal, Guidance requires reference waypoint input from the 
Mission Plaimer and current vessel parameters from the Navigation sub-system. 

•Reference’ parameters are those received directly from the Mission Planner. 
'Current' paramters are values which are calculated by Navigation during the most 
recent iteration. 'Desired' parameters are those values fed to the Autopilot after 
processing by Guidance. More complete definitions of these terms can be found in 
Appendix E. 

A. INPUTS AND OUTPUTS 

The Guidance Subsystem provides local path-planning and tracking control 
for AUV II. It receives as input a file of waypoints consisting of three dimensional 
grid position and velocity (x,y,z,v) from the Mission Planner. These waypoints are 
used to construct a smooth path between waypoints. This smooth path is broken into 
a series of discrete postures (x,y,z,0,0,v) which are then output to the Autopilot, one 
posture at a time. 
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B. SUBSYSTEM COMPOSITION 


The Guidance Subsystem for AUV II is comprised of three major 
functional parts: 

(1) OFF-LINE LOCAL PATH PLANNER - an off-line program (see Appendix 
F for source code) which extracts and stores desired curve information for all 
allowable combinations of waypoints. 

(2) ON-LINE LOCAL PATH PLANNER - on-line program (see Appendix G 
for source code) that steps through a given cubic spiral, providing reference 
postures to the tracking controller at a user specified frequency. 

(3) TRACKING CONTROLLER - on-line program (also in Appendix G) 
which provides navigational corrections to the reference postures from the on¬ 
line local path planner to create desired postures. The desired postures are 
fed to the Autopilot. 

Each component of the Guidance subsystem is described in detail in a subsequent 
portion of this chapter. 

The Guidance system algorithms presented herein perform local-path nlanning 
and Tracking Control fo* two-dimensions. Application to the third dimension (depth) 
is discussed at the end of this chapter. 


C. INTERFACE WITH OTHER SOFTWARE MODULES 

It is useful to divide the computer systems onboard an autonomous vehicle into 
individual modules which perform specific functions. This approach has been 
followed in a number of AUVs and Remotely Operated Vehicles (ROVs) 
[CROWLEY 85, ALBUS 90, RUSSELL 86, BELLINGHAM 90, ZHENG 90]. For 
AUV II, the onboard software is divided into the following Modules: 
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• Mission Replanning 

• Navigation 

• Collision Avoidance 

• Obstacle Recognition 

• Local Path-Planning (LPP) 

• Autopilot 

• Data Collection 

• Hovering 

The inter-relation of these modules is illustrated in Figure 10, Three of these 
modules/subsystems interact directly with the Guidance subsystem: 

• Mission Planner 

• Navigation 

• Autopilot 

The function of these modules and their interaction with the Guidance subsystem are 
described below. 

1. Mission Planner 

The Mission Planner is responsible for finding a safe path between the 
starting position and the goal position utilizing the information available from the off¬ 
line world model. The path must minimize distance while at the same time taking 
into account other factors such as mission duration, threat analysis, and other possible 
hazards. The output of the mission planner must be readily usable by the onboard 
computer systems. 
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Figure 10 - AUV II Software Modules 
Because the paths that an autonomous vehicle must follow are complex 
and subject to change, real-world data must be obtained and utilized to arrive at 
satisfactory solutions [MOZIER 87]. AUV II will contain vehicle models and mission 
models at various levels, and will have on-line environmental databases to ensure 
proper data input for all of the above modules [HEALY 90]. 

The choice of an appropriate global or world model is essential to 
designing and implementing a successful navigation system and as a consequence 
most mobile robot planners are strongly influenced by the world modeling method 


employed. In addition, selection of appropriate information to be passed to the 
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vessel can greatly simplify the design of the Mission Planner. "The key to a successful 
modelling technique lies in defining a conceptual model that adequately describes the 
operating environment for the tasks being performed and < which > lends itself to 
computer implementation...." [MOZIER 87] By utilizing a grid structure for the world 
model, formal search methods such as A* and depth-first can be employed in 
conjunction with appropriate heuristics. The current Mission Planner for AUV II is 
based on a three-dimensional rectangular grid and uses several different heuristic 
search methods [ONG 90]. 

a. Grid Size Determination 

The grid has identical x and y dimensions, with a smaller z dimension. 
A two-dimensional representation of this grid is illustrated in Figure 11. For the 
purposes of software testing he grid is sized based on estimated vessel 
maneuverability. The actual values must be validated during initial trials. 

The Mission Planner assumes that the AUV is traveling on a three- 
dimensional grid. The algorithm utilized by the Mission Planner in calculating a 
desired path assumes that the AUV can only traverse directly from one waypoint to 
another waypoint on the grid (it cannot stop at intermediate points off of the grid), 
and that when it arrives at the next waypoint, it will be heading in a cardinal direction 
(North, South, East or West). This type of maneuver is illustrated in Figure 12. The 
position and direction assumptions are the critical link between the algorithms used 
by the Guidance Subsystem and the path prepared by the Mission Planner. 


44 






Figure 12 - Allowable Maneuvers 
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The basic size for the grid is established during sea-trials. Basic grid 
size is the maximum of the x and y values (advance and transfer) obtained as the 
vehicle executes a SK)° course change under worst case conditions (most probably at 
low speed). 

b. Output from Mission Planner 

The output from the Mission Planner is a series of waypoints (x,y,z,v) 
which describe the optimal path and desired velocities to be achieved between the 
start and goal points. Note that this path is optimal only for the given set of 
boundary conditions. The Grid size is determined off-line based on the AUVs ability 
to execute a 90 degree course change under worst case conditions. For example, if 
the worst case calculations showed a change in x of 70 cm and a change in y of 90 cm 
in 4 sec. Also assume that the worst case expected current resulted in a set of 10 
cm/sec. The maximum dimension would be 130 cm (90 cm plus 10 cm/sec for four 
seconds). The resulting grid would be 130 cm on a side. These waypoints are 
coupled with reference vehicle velocity to form a file of waypoint-velocity pair which 
is downloaded to AUV II. Since the Mission Planner has knowledge of possible 
obstacles in the AUVs world, the area between these waypoints is assumed to be 
obstacle free. 

The Mission Planner can also be programmed to correct for expected 
set and drift encountered as the vehicle maneuvers. This capability will be especially 
beneficial when the AUV is operated in the bay or at sea. As an example, assume 
that the worst case measured values for advance and transfer are 100 cm and 80 cm 







respectively, and that these values were obtained at a vehicle velocity of 0.5 
meter/second. Using these values, the basic grid size would be 100 cm. If the 
expected set and drift was 0.1 meter/second (direction is irrelevant), then the 
correction is given by: 


corr = 


Gridjz 

2 

trml_yel 


(4-1) 


and the final grid size would be 130 cm (including a correction of 30 cm). The 
proposed x, y, and z positions that the Mission Planner outputs to Guidance are 
integral values of the final grid size. 

2. Autopilot 

The Autopilot receives desired waypoint, heading and velocity values from 
Guidance. These desired values are reference values which have been corrected 
based on current vehicle parameters [RILING 90]. The Autopilot outputs commands 
to the control surfaces, and main motors as it tries to achieve the desired attitude. 

3. Navigation 

The Navigation subsystem determines current vehicle position, heading, 
velocity and acceleration and provides this information to Guidance for comparison 
with reference waypoint-velocity information and deduced reference velocity and 
acceleration values [FRIEND 89], 
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D. OFF-LINE LOCAL PATH PLANNER 

FIND_CS is the off-line program which performs the cubic spiral calculations 
for Local Path Planning in the Guidance subsystem. It is a modification of the cubic 
spiral calculation program developed by Kanayama for Yamabico-11 [KANAYAMA 
88a]. 

The program takes as input a file consisting of the 27 possible combinations of 
X and y positions in the xy-plane. FIND_CS takes these 27 sets of data, creates two 
postures Pj and P 2 , determines the appropriate cubic spiral (or pair of cubic spirals) 
which joins these two postures, and writes the associated cubic spiral data to the 
output file. The position combinations are four sets of xy-pairs {(Xj,y 2 ),(x 2 ,y 2 ), (X 3 ,y 3 ) 
and (X 4 ,y 4 )} which are used to calculate Pj and P 2 as described below, 

1. Reference Heading 

Each posture is calculated using three of the pairs. Pj is calculated using 
the first three xy-pairs {(Xi,yj),(x 2 ,y 2 ), and (x 3 ,y 3 )}, and P 2 is calculated from the last 
three xy-pairs {(X 2 ,y 2 )j(x 37 y 3)5 and (X 4 ,y 4 )}. The xy-position for the posture is taken 
from the inner xy-pair of the triple (for Pj the xy-position would be (X 2 ,y 2 ) and for P 2 
it would be (X 3 ,y 3 )). The reference heading for each posture is calculated in the same 
manner as was described earlier, where heading is the arc tangent of the difference 
between the previous and the next positions on the grid. As an example, if the input 
pairs are ((0,0)(1,1)(2,2)(2,3)), then the Pj and Pj positions would be (1,1) and (2,2) 
and the associated angles would be: 
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(4-2) 


and 


' ( 2 - 0 ) 


45“ 


0 = tan-‘^^-^ = 63.43“ (4-3) 

' ( 2 - 1 ) 

respectively. 

2. Cubic Spiral Calculation 

Once the postures have been determined, the next step is to determine the 
appropriate cubic spiral or cubic spirals to join the waypoints in the same manner as 
Yamabico-11, specifically: 

(1) If Pj and Pj are symmetric, then find the cubic spiral which joins Pj and Pj. 


(2) If Pj and Pj are not symmetric, then find the split posture (Q) between Pj and 
Pj such that the sum of the cost for (Pj.Q) and (Q,P 2 ) is minimum. 

(3) Find the cubic spiral joining Pj and Q, and then do the same for the cubic 
spiral joining Q and Pj. 

After the appropriate cubic spirals have been found the associated length 
(/) and curvature constant (a) values are written to the output file. The curvature 
constant is: 


Both of these terms and their importance to the local path-planning calculation are 
described fully in [KANAYAMA S6a]. 
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E. ON-LINE LOCAL PATH PLANNER 

The local path-planner must find a smooth path firom waypoint to waypoint. 
In addition to waypoint information, the desired vehicle heading at each waypoint 
should also be included in the calculation, and can be inferred from waypoint 
information. 

The desired vehicle heading at a waypoint is taken to be the arc-tangent of the 
two dimensional Cartesian difference between the previous waypoint (Xj, yj) and the 
next waypoint (Xj, y 2 ): 

0 = . (4-5) 

Figure 13 illustrates this calculation in the case where the prior waypoint and the 
current waypoint have the same x grid values, and the following waypoint has x and 
y values one grid unit larger resulting in a desired heading of 27.5 degrees. 

A posture (x,y,z,v,0) is defined as a combination of the Cartesian coordinates 
of a point, a velocity, and a heading. Two postures are required as input to the local 
path-planner. The output from the local path-planner is a smooth "path" which 
satisfies the input conditions while minimizing total cost. A path (it) can be 
represented as a function of length (s) along the path where the tangent direction (0) 
and curvature (k) of tt are: 
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Figure 13 - Calculation of Desired Heading 


0(s)= tan 


-1 


ds 

ds 


k(s)= — . 
ds 


(4-6) 


(4-7) 


1. Cardinal Heading Maneuvers 

The vehicle is initially assumed to be heading in a cardinal direction 
(North, South, East or West). The 'Cardinal Heading’ is calculated by the 
Guidance subsystem based on the first three waypoints. This calculation, and its 
effect on Guidance, is explained below. 


With the stipulation that the vehicle can only turn left (L), go straight (C), 
or turn right (R), and that the vehicle must end up on the grid, all possible 











combinations of vehicle maneuvers can be reduced to a set of twenty-seven curves, 
each of which can be identified by a triple (e.g. LLL) as specified in Table V. In 
Figure 14, the vehicle was initially heading East, then turned to the North (L), the 
West (L) and then to the South (L), for a combined (ULL) maneuver. The resultant 
cubic spiral is a quarter circle to the left. 

Table V - Possible Combinations of Maneuvers 



Any maneuvers which require more than these three changes in cardinal 
heading can be simulated through some combination of these twenty-seven triples. 

Since there are only twenty-seven predefined possible maneuvers, the cubic 
spiral information is precalculated using Kanayama’s method [KANAYAMA 88a], and 
placed into a lookup table, resulting in a tremendous increase in calculation speed. 
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Figure 14 - Example of a LLL Maneuver 


2. Theory of Operation 

rhe objective of the LPP is to provide appropriate data to the Stepper 
which calculates the next desired posture to follow between waypoints. The Stepper 
requires information on the length of a curve (d) and curve deflection (a), where d 
is the Euclidean distance between the waypoints and a is the angle between the 
desired vehicle heading at each of the waypoints. This pair (d,a) is called a curve set. 

On Yamabico-11, the local path planner must perform curve set 
calculations on-line, since the waypoint orientation is unknown [KANAYAMA 89a]. 
For AUV-II the task is much simpler. The local path calculations are expedited by 
using a stored curve database which contains all allowable combinations of vehicle 
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maneuvers. By eliminating on-board calculations of curve parameters, the real-time 
response of the LPP is dramatically improved. 

Because of the simplified set of twenty-seven possible combinations of 
waypoints (as indicated in Table V) achievable by the AUV, the curve set 
information is calculated by the off-line local path planner and is placed into a lookup 
table for use by the on-line local path-planner. The off-line program utilizes path 
curvature and the derivative of path curvature as cost functions as follows: 

cost = * D(a)^ . 

As specified by Kanayama, there is only one simple path with a given curve 
set {d,oi)[KANAYAMA 88a]. If a set of waypoints (Xj,yi) (X 2 ,y 2 ) is symmetric, then the 
desired curve set consists of a single cubic spiral. The desired solution to a set of 
waypoints is symmetric if and only if 

0j- P = P - 02 (4-9) 

where 

p = tan-^( ^^ ~ -). (4-10) 

Xj - Xj 

If the waypoint pair is not symmetric then two curves which satisfy the 
minimum cost function are required, resulting in two curve sets. The lookup table 
utilized by the local path-planner has two curve sets for each of the possible 
combinations, with the second curve set placed to zero if the waypoint pair is 
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symmetric. These lookup table are size-free and can be scaled to any desired grid 
size. 

Once a set of curve information has been retrieved, STEP_SPIRAL is used 
to calculate a series of intermediate reference postures which are used by the 
Tracking Controller. A reference posture is defined as a quadruple of reference 
x-position, y-position, depth, speed and heading (x,y,z,v,0). 

3. Waypoint Processing 

As waypoints are passed to the local path-planner, a new vessel pseudo¬ 
heading is computed based on the past way-point (Xo,yo) and the current way-point’s 
successor (X 2 ,y 2 )- This pseudo-heading is used to calculate relative direction change 
and the new vessel cardinal heading. The new pseudo-heading (0j) is given by: 

0 = tan'f^) - (4-11) 

where i|ro is the cardinal heading which existed prior to the maneuver. Using 
Figure 14 as an example again, assume the vessel was initialially heading East, the 
pseudo-heading to the next waypoint would be -90.0°, resulting in a relative direction 
change of (L), and a new cardinal heading of North. This relative dirction change 
is combined with the last two relative direction changes to form a new triple. The 
triple is used as the table entry for the table lookup of the appropriate cubic spiral 
curve information. 

The desired vehicle heading calculation at the start of the mission is 
somewhat more complex. Since there is no previous waypoint, the initial reference 
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heading must be calculated based on the following two waypoints. This is 
accomplished by determining whether the change in the x and y directions are 
positive, negative or zero. Two pairs (sign-x sign-y) are calculated and reference 
vehicle heading and dummy values for the previous position (X(„yo) are chosen based 
on these pairs. 

4. Reference Value Calculation 

Reference values are dead-reckoned from the point where the mission 
originated. The length of a "step" from one reference posture to the next is current 
vessel velocity multiplied by execution time interval. 

Let do denote the distance of a point from the start of the path, and s the 
size of an incremental change in cIq. Then the distance after an incremental change 
becomes: 

d, * do + j (4-12) 

and the curvature k can be determined from: 

K = A * * (I - d^) (4-13) 

where / is the total length of the cubic spiral. The change in the reference heading 
(A0r) is then: 

AS, = K * s (4-14) 

and the new reference heading is: 
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(4-15) 


0, = + Ae . 


The new reference values for x and y (x^^) are calculated from simple trigonometric 
relationships: 


= cos(e^ ♦ s 

(4-16) 

= sin(6^) * s . 

(4-17) 


F. TRACKING CONTROLLER 

The purpose of the Tracking Controller is to provide desired postures to the 
Autopilot, The Tracking Controller for AUV II is modelled after the system utilized 
in Yamabico-11 by Kanayama because of its proven capability, robustness and ease 
of implementation [KANAYAMA 90]. 

1. Basic Operation 

The Tracking Controller receives as input reference postures and velocities 
from Step_Spiral which define the path that the vessel should follow, along with 
actual position and velocities from Navigation which show the actual vehicle path. 
An error value calculation is performed and applied to the reference position to get 
the desired values of heading, velocity and position to provide to the Autopilot. 

The AUV in the world possesses six degrees of freedom in positioning 
which are represented by a posture (x,y,z,0,d>,ijr), which is also a funciion of time. The 
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set of positions (x,y,z) that the vessel traverses is called a path, and it can easily be 
shown that heading is actually: 


e = (4-18) 

X 

(where the dot is defined to be the derivative with respect to time). 

The velocity of the vehicle in three-dimensional space can be specified as: 

a triple: 

^=(v,6),t|r) (4-19) 

where v is the AUV speed, <u is rotational velocity in the x-y plane and is the 
rotational velocity in the x-z plane (we are neglecting vehicle roll). 

The relationship between the posture p and the velocities q is given by: 


q = (v,<o,<i)) = (i cosS cos<}> + 3 ? cos 0 sin<l), 0 , (j)) 
where 6 is the current vehicle heading and (f) is the current vehicle pitch. 


(4-20) 


2. Error Value Calculation 

The error value determination method used in AUV II is identical to that 
used in Yamabico-11. The error values are the difference between reference and 


actual positions scaled by actual heading and are calculated from: 

= (^r - ^c) * «)s(0^) + (y, - y^) * sin(0^ 
Ve = (^f ■ * sin(0^) + (y, - y^) * cos(0^) 

0 . = 0 , - 0 , . 


(4-21) 

(4-22) 

(4-23) 
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3. Desired Value Calculation 

As the vehicle proceeds from waypoint to waypoint, it is constantly 
comparing actual posture (P^) to reference posture (P^). There are three possible 
outcomes from this calculation: 

• The vehicle is ahead of the reference posture (x^ positive). 

• The vehicle is at the reference posture (x^ approximately zero). 

• The vessel is behind the reference posture (x^ negative). 

If \ is positive, a new reference posture is calculated by the on-board LPP, desired 
velocity is decreased, and new x^ and y^ values are calculated and passed to Autopilot. 
If Xg is nearly zero, a new reference posture is calculated by the on-board LPP, v^ 
remains unchanged and a new desired posture is calculated and passed on to 
Autopilot. If the AUV is lagging behind (x,. negative), then the reference posture 
remains the same, x^ and y^, are recalculated, and v^ is increased and passed on to 
Autopilot. 

For AUV II, the desired x and y values are calculated using the same 
equations developed for Yamabico-11, and are then scaled to the current vessel grid 
size: 

Xj = (Jc, + x^) * GRID_SZ (4-24) 

= (^r + y,) * GRIDJZ (4-25) 

where the error values and reference values are calculated as above. 







Desired velocity determination is also the same as for Yamabico-11: 

= (K^* + (v^* 0^ . (4-26) 

The calculation of desired heading is somewhat more complex. Since o is 
the derivative of heading, the desired change in heading can be obtained by: 

A0 = <0^ + * (jKy*y^ + (4-27) 

where o),. and v^ are the actual angular velocity and linear velocity supplied by the 
Navigation subsystem. This change in heading (A0) is added to the old desired 
heading to determine the new desired heading. Ky and are integration 
constants whose values are calculated using the methods specified by Kanayama 
{KANAYAMA 90]. For the purposes of validating the Guidance Subsystem software 
without a vessel, these constants were calculated to be: = 10/sec, Ky = 

0.0064/cm^, and = 0.16/cm. The final values for these constants must be 
determined and verified by experimentation when the vehicle is in the water. 

4. Look-Ahead Limitation 

The Tracking Controller is designed to "drag" AUV II along from desired 
posture to desired posture. It is desirable for the next desired posture to be just far 
enough ahead of the current posture that the vehicle will reach the desired posture 
after a certain number of cycles of Autopilot operation. For the Tracking Controller 
in AUV II the Autopilot is allowed ten cycles to achieve the desired posture. 

The selection of the factor of ten difference in the timing between the 
Tracking Controller and the Autopilot is quite common in AUV control [BLIDBERG 
90, ALBUS 90], and provides for adequate separation of the various layers of control. 
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If the vehicle is unable to achieve the desired position in ten cycles, then 
a decision must be made either to calculate a new desired position, or to continue to 
the old desired position. This is accomplished by comparing the size of to a 
reference maximum value. As long as x^ is negative and less than the maximum, a 
new reference value is calculated. If x^ becomes greater than the maximum, the old 
reference posture is passed to the Autopilot. 

If the vehicle gets ahead of the desired posture, a new posture is 
calculated, and in addition, the vehicle velocity is decreased by an amount 
proportionate to the size of x^. 

G. TESTING RESULTS FOR TWO-DIMENSIONAL GUIDANCE 

The two-dimensional Guidance subsystem source code was compiled and 
installed on the GESPAC Vehicle Control Computer. The output of the Guidance 
subsystem was written to a file along with the waypoints and the smooth cubic spiral 
curves between waypoints. Since there is neither an operational navigator nor a valid 
simulation model for AUV II, the last desired parameters were fed back as actual 
vehicle parameters to Guidance. This artificiality is not a major concern, because the 
Tracking Controller system has already been operationally tested in Yamabico-11 
[KANAYAMA 89]. 

Several files of waypoints generated by the Mission Planner were downloaded 
to the GESPAC via an RS-232 serial port, and the missions were run to completion. 
All missions resulted in adequate vehicle performance. 
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The results of one such mission are shown in Figure 15. The path followed by 
Step Spiral is the solid line. The waypoints provided by the Mission Planner are the 
diamonds. The desired postures fed to the Autopilot are the crosses. As can be seen 
from this drawing, the Tracking Controller was able to "drag" the simulated vehicle 
along with no discemable error. 

H. THREE-DIMENSIONAL CALCULATIONS 

As was previously mentioned, the Guidance subsystem described thus far only 
performs local path-planning and Tracking Control in two-dimensions. The addition 
of depth brought new and interesting problems. 

1. Depth Descriptors 

Since the vehicle was operating on a three-dimensional grid, it was possible 
to create a set of curve descriptors which would apply to depth. The vehicle 
isrestricted to moving ahead, up or down just one grid square at a time (U,S,D). 
These three possiblities are applied in the same manner as for the two-dimensional 
case, and once again result in a maximum of 27 possible combinations of cubic spiral 
curves. 

The primary problem here is that unless the z grid dimension is the same 
as the X and y dimensions (resulting in a cubic grid pattern), then the depth curve 
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Figure 15 - Guidance Subsystem Performance 


descriptors will have to be chosen based on the scale difference. As an example, if 
the x/y dimension is 100 cm and the z dimension is 10 cm, then the depth curves must 
be calculated based on this relationship. The program FIND_D (see Appendix H for 
three-dimensional source code) performs these calculations. It requires an input file 
showing the relationship between the waypoints, and outputs an appropriate set of 
cubic .spiral curve descriptors. 
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2. Modifications to On-Line Local Path Planner 


The vehicle must travel from waypoint to waypoint on the three- 
dimensional grid, and it must arrive at the x-y waypoint coordinate at the same time 
as it arrives at the z waypoint coordinate. This poses the hardest problem in 
achieving three-dimensional guidance. There must be some time synchronization 
between stepping through the x-y curve set and the z curve set. 

The total distance to travel in each dimension is known (it is the sum of the 
lengths of the individual cubic spirals (1)) and the number of steps in each dimension 
must remain equal. From this it is easy to see that to travel the same number of 
steps, and yet go a different distance, the step sizes must be different. Therefore, the 
z step size is calculated from: 


zjstep - xy_step * 


(xy.lenl +xy.len2) 


(4-23) 


Q:-lenl*z.Un2) 

By keeping the number of steps the same, the time spent in each curve will be 
identical, and the vessel will complete both curves at the same time. 
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VI. CONCLUSIONS AND RECOMMENDATIONS 

This chapter summarizes the contributions which have been made to AUV 
research in general, and specifically to the NFS AUV 11. It also provides 
recommendations for future research in AUV II guidance and control. 

A. CONTRIBUTIONS 

1. Vehicle Control Computer System 

The necessary computer equipment was identified, procured and 
configured to ensure that all identified data processing and data translation 
requirements at the Vehicle Control level were met or exceeded. In addition, the 
chosen VCC allows for data translation capability upgrades, and for expansion to 
multiple processors at some future date. 

2. Guidance Subsystem 

A functional Guidance subsystem has been implemented and tested on the 
Vehicle control computer. This subsystem calculates a smooth path from waypoint 
to waypoint while minimizing vessel jerk. The Tracking Controller in this subsystem 
provides all necessary information to the Autopilot at a user specified frequency. 

The Tracking Controller module is robust enough to be used to provide 
vehicle control commands without an Autopilot. 
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3. Vehicle Firmware 


All data translation firmware necessary for the Vehicle Control Computer 
has been designed, implemented and tested. This software should be able to function 
totally unchanged for the life of the project. 

In addition real-time software has been developed and tested for the main 
motors and hydrodynamic control surfaces. The 'lookup table’ function used to 
control these effectors and the algorithm used in creating the lookup tables for these 
devices can be applied to all other vehicle control mechanisms. 

This combination of firmware provides all software necessary for initial in¬ 
water testing of AUV II. 

B. RECOMMENDATIONS FOR FUTURE WORK 
1. Real-Time scheduling 

The single most important decision facing the AUV II real-time computer 
systems is the design and implementation of a multi-tasking scheduling scheme. The 
OS-9 operating system is designed to allow the use of several possible methods of 
multi-tasking [DIBBLE 55]. The event driven control method used by ISE 
[ZHENG 90] is another viable option, as is the real-time pre-emptive scheduling 
methods being pursued by MIT-Sea Grant [BELLINGHAM 90]. This scheduling 
decision will drive the design and implementation of all other support modules. 
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2. Guidance Subsystem 

The Guidance subsystem should be linked to the off-hull Mission Planner 
to allow for accurate real-time simulation of mission plans. This integration, 
combined with the integration of Autopilot and Navigator, and an improved vehicle 
dynamic model, will allow missions to be run in real-time, which would greatly 
enhance the credibility of the vehicle simulatons. 

The Guidance subsystem must be interfaced to the Autopilot when the 
Autopilot has been operationally tested on the Vehicle Control Computer. Further 
testing of the Guidance subsystem should also be accomplished using simulated 
vehicle dynamic inputs to verify the robustness of the Tracking Controller. 

3. Communication 

There is an urgent need to establish real-time communications between 
the GESPAC and the GRiDCASE computers. Waypoints must be sent from the 
GRiDCASE to the GESPAC, and vehicle attitude and positional information must 
be sent from the GESPAC to the GRiDCASE to allow for Mission Replanning. In 
order to facilitate initial vehicle testing, an RS-232 link should be connected between 
the GESPAC and the pierside GRiDCASE/PC. The removable fiber-optic data link 
would be the best choice for this communications connection. 

4. Vehicle Firmware 

Routines must be written which will ensure that equipment in AUV II is 
powered up in the proper order, and which will ensure proper operation of the 
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effectors prior to launch. In addition, firmware is required for the inertial sensor 
suite, for the health and well-being sensors, and for the tunnel thrusters. 
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APPENDIX A. GRiD TIMED INTERRUPT SOURCE CODE 


Program: TEST TSR.C 

Purpose: timed interrupt test program for GRiDCASE 1535 EXP 
Author: LT M.J. CLOUTIER 


TEST_TSR does the following; 


1. Using vector 66h, checks whether the clock is already installed 

2. Saves original clock vector 

3. Sets up new clock vector (Install_Autopilot) 

4. Does a "do nothing loop" to evaluate the TSR function 


V 


(NOTE: This code is an extensive modification of a timer routine in the DOS 
Programmers Reference by DETTMAN) 


#include <stdio.h> 
#include <dos.h> 
#include <conio.h> 


/* constants */ 

#define PGMSIZE 3000 

/* define base address for video display */ 

#define MONOBASE OxbOOO 

#define COLORBASE OxbSOO 

#define EGABASE OxaOOO 

/* interrupt vectors for BIOS and DOS */ 

#define AUTOPILOT 0x66 

#define TIMER Oxlc 

#define VIDEO 0x10 

void interrupt (*orig_clock)(void); /* original clock vector */ 
void interrupt autopilot(void);/* declare autopilot() *! 
void Install_Autopilot(void); 
void Remove_Autopilot(void); 
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int tick_count = 0; /* clock tick counter 

int sp; /* stack pointer *1 

int ss; /* stack segment */ 

int count = 0; 

char buf[80] = "<- 

char far ^auvptr; 

main() 

{ 

union REGS regs; 
int mode; 

int j; 

Instal]_Autopilot(); 
for(j=0 ;j < 20;j ++){ 

printf("Count=%d\n",count); 
delay(50); 

} 

Remove_Autopilot(); 

} 

void Install_Autopilot(void) 

/* Set up the Autopilot interrupt handler, saving machines clock program to 
orig_clock */ 

{ 

int mode; 

/• sets auvptr for appropriate screen mode */ 
mode = getmode(); 
printf("Display mode is %d\n",mode); 
if (mode ==7) 

auvptr = MK_FP(MONOBASE,3856); 
else if (mode = =3) 

auvptr = MK_FP(COLORBASE,3856); 

else 

auvptr = MK_FP(EGABASE,3856); 
orig_clock = getvect(TIMER); 
setvect(TIMER,autopilot); 

} /* Install_Autopilot */ 

void Remove_Autopilot(void) 

/* Removes Autopilot interrupt handler, restores machines clock program */ 

{ 


70 









setvect(TIMER,orig_clock); 

} /* Remove_Autopilot */ 

void interrupt autopilot(void) 

/* Autopilot interrupt handler ... called by the timer interrupt 
18.2 times per second. Every 5th second add one more tick 
before advancing to keep average time correct 
V 
{ 

static int INSIDE_AUTOPILOT = 0; 
static unsigned long Count = 0; 

/* call the original interrupt first */ 

(*orig_clock)(); 

/* Advance the tick counter */ 
tick_count++; 

/* Every 20th tick write Autopilot string */ 
if (!INSIDE_AUTOPILOT && !(tick_count ^ 0x0a)){ 
tick count = 0; 

INSTdE_AUTOPILOT = 1; 
disableO; 
sp = _SP; 
ss = _SS; 

_SS = _CS; 

_SP = PGMSIZE; 
enable(); 

itoa(+ +count,&buf[8],10); 
displaystr(buf); 

for (;Count < 400000;Count+4-); 

disable(); 

_SP = sp; 

_SS = ss; 
enable(); 

Count = 0; 

INSIDE_AUTOPILOT = 0; 

} 

} /* autopilot */ 
displaystr(str) 
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I* Display the string at the clkptr position on the screen */ 
char *str; 

{ 

char far *ptr; 

ptr = auvptr; 
whiJe(*str){ 

*ptr++ = *stri-+; 
ptr++; 

} 

} /*displaystr *! 
getmodeO 

r Returns video display mode of the system */ 

{ 

union REGS regs; 

regs.h.ah = OxOf; 

int86(VIDEO,&regs,&regs); 

retum(regs,h.al); 

} /* getmode *! 
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APPENDIX B. DATA TRANSLATION SOURCE CODE 


I* 

* Program ™ ALL_ADDA.C 

* Purpose — Data Translation routines for GESPAC DAC2B, ADAl and 

ADC2C * cards for AUVII 

* Author — LT M.J. CLOUTIER 

* (Some of these routines are from data translation routines used in the MIT Sea 
Sprite) 

V 

#define DAC_ADDR OxFFFOOOOO 

#define DAC0_MSB 0x0 

#define DAC1_MSB 0x4 

#define DAC2_MSB 0x8 

#define DAC3_MSB Oxc 

#define DAC_LSB_OFFSET 0x2 

#define ADC_ADDR (DAC_ADDR -I- 0x11) 

#define ADC_MSB 0x0 

#define ADC_LSB 0x2 
#define ADC_CMD_REG 0x4 
#define ADC_STATUS_REG 0x4 
#define ADC_BUSY 0x4 

#define DAC2B_ADDR 0xFFF00040 
#define DAC4_MSB 0x10 

#define DAC5_MSB 0x14 

#define DAC6_MSB 0x18 

#define DAC7_MSB Oxlc 

#define ADC2_ADDR 0xFFF00020 

#define ADC2_CH_GAIN 0x0 
#define ADC2_STATUS_REG 0x2 
#define ADC2_DATA 0x1 

#define ADC2_CMD_REG 0x2 

unsigned short *adc2_a = ADC2_ADDR; 
unsigned char *dac2b_a = DAC2B_ADDR; 
unsigned char *adal_dac_a = DAC_ADDR; 
unsigned char *adal_adc_a = ADC_ADDR; 
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* adal_adc(n) -- reads adc channel ’n’ (channels 0-15) 

int adal_adc(n) 
int n; 

{ 

int val; 

adal_adc_a[ADC_CMD_REG] = n; 

while (adal_adc_a[ADC_STATUS_REG] > 20); /* wait for data */ 

val = adal_adc_a[ADC_MSB] << 2; 
val += adal_adc_a[ADC_LSB] >> 6; 
return (val); 

} /* adal_adc */ 

* adc2_adc(n) - reads adc channel ’n’ (channels 0-15) 

* with gain ’g’(0 to F = > 0 - 1024) 

********************************************************I 

int adc2_adc(n,g) 
int n,g; 

{ 

int val; 

adc2_a[ADC2 CH_GAIN] = (n < < 4) | g; /* set c&g, start conv */ 
while (adc2_a[ADC2 STATUS_REG] != 0); /* wait for ready V 
val = adc2_a[ADC2jDATA]; 
return (val); 

}/* adc2_adc */ 

* adal_dac(s,ch) - writes signal ’s’ to adal dac channel ’ch’ 

* (allowable channels 0-3) 

*m**************************************’¥********* ****** I 

void adal_dac(s,ch) 
int s,ch; 

{ 

ch = ch < < 2; /* offset for G-96 addressing */ 

adal_dac_a[ch] = s > > 2; /* write upper 8 bits to MSB*/ 

adal_dac_a[ch -f- DAC_LSB_OFFSET] = s << 6; /* write lower 2 bits B3,B2 */ 
return; 

}/* adal dac */ 
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* dac2_dac(s,ch) -- writes signal ’s’ to dac2 channel ’ch’ 

* (allowable channels 0-7) 

void dac2_dac(s,ch) 
int s,ch; 

{ 

ch = ch < < 2; /* offset for G-96 addressing */ 

dac2b_a[ch] = s > > 4; /* write upper 8 bits to MSB*/ 

dac2b_a[ch + DAC_LSB_Of FSET] = s < < 4; /* write lower 4 bits B3-B0 */ 
return; 

}/* dac2_dac */ 

* read_adal_dac(ch) -- read output from dac 

int read_adal_dac(ch) 
int ch; 

{ 

int s; 

ch = ch << 1; 
s = Edal_dac_a[ch] << 2; 

s = s + (adal_dac_a[ch f DAC_LSB_OFFSET] >> 6); 
return (s); 

}/* read_adal_dac */ 

^^:t,^,*^m********************************>l’-********** ****** 

* adal_2_adal(nl,n2) -- signal from adal adc(nl) written 

* to adal dac(n2) 

*********************************************************^ 

void adal_2_adal(nl,n2) 
int nl,n2; 

{ 

int val; 

n2 = n2 << 2; 

adal_adc_a[ADC_CMD_REG] = nl; 

while (adal_adc_a[ADC_STATUS_REG] > 20); /* wait for data */ 

adal_dac_afn2] = adal_adc_a[ADC_MSB]; 

adal_dac_a[n2 + DAC_LSB_OFFSET] = adal_adc_a[ADC_LSB]; 

}/* adal 2 adal */ 
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* adc2_2_adal(nl,n2) - signal from adal adc(nl) written 

* to adal dac(n2) 

void adc2_2_adal(nl,g,n2) 
int nl,g,n2; 

{ 

unsigned short val; 
n2 = n2 << 2; 

adc2_a[ADC2_CH_GAIN] = (nl << 4) | g; 

while ((adc2_a[ADC2_STATUS_REG] & 0x7) != 0); /* wait for data */ 

val = adc2_a[ADC2_DATA]; 

adal_dac_a[n2] = val >> 4; 

adal_dac_a[n2 + DAC LSB OFFSET] = val << 4; 

}/* adc2_2_adal ”"7 

* adal_2_dac2(nl,n2) -- signal from ada adc(nl) written 

* to dac ch(n2) 

void adal_2__dac2(nl,n2) 
int nl,n2; 

{ 

n2 = n2 << 2; 

adal_adc_a(ADC CMD_REG1 = nl; 

while (adal_adc_a[ADC_STATUS_REG] > 20); /* wait for data */ 

dac2b a[n2] = ^al_adc_a[ADC_MSB]; 

dac2b~a[n2 + DAC_LSB_OFFSET] = adal_adc_a[ADC_LSB]; 

}/* adal_2_dac2 */ 

y(* ♦♦*♦♦♦♦♦♦***♦♦*>»*♦*♦♦***♦♦♦♦>•■**♦♦*♦♦«♦♦♦**♦♦♦*♦******♦ 

* adc2_3_dac2(nl,n2) -- signal from ada adc(nl) written 

* to dac ch(n2) 

void adc2_2_dac2(nl,g,n2) 
int nl,g,n2; 

{ 

unsigned short val; 
n2 = n2 << 2; 

adc2_a[ADC2_CH_GAIN] = (nl << 4) | g; 

while ((adc2_a[ADC2_STATUS_REG] & 0x7) ! = 0); /* wait for data */ 
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val = adc2_a[ADC2_DATA]; 

dac2b_a[n2i = val >> 4; 

dac2b_a[n2 + DAC LSB_OFFSET] = val << 4; 

}!* adc2_2_dac2 */ 

main (argc,argv) 
int argc; 
char *argv[]; 

{ 

int val; 

/* This program reads from the ADAl or ADC2B ADC and writes to the DAC2B 
DAC * to test the data conversion routines 
V 

printf("ALL A2D && D2A: testing\n"); 
while (1){ 

r 

adal_2_dac2(0,0); 
adal_2_dac2(l,l); 
adal_2_dac2(2,2); 
adal_2_dac2(3,3); 
adal_2_dac2(4,4); 
adal_2_dac2(5,5); 
adal_2_dac2(6,6); 
adal 2 dac2<'7,7); 

*! 

adc2_2_dac2(0,0,0); 
adc2_2_dac2( 1,0,1); 
adc2_2_dac2(2,0,2); 
adc2_2_dac2(3,0,3); 
adc2_2_dac2(4,0,4); 
adc2_2_dac2(5,0,5); 
adc2_2_dac2(6,0,6); 
adc2_2_dac2(7,0,7); 
adc2_2_ada 1 (8,0,0); 
adc2_2_ada 1 (9,0,1); 
adc2_2_ada 1(10,0,2); 
adc2_2_ada 1( 11,0,3); 
adc2_2_ada 1(12,0,4); 

} 

} r ALL_ADDA */ 
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APPENDIX C. SERVO CONTROL CODE 


Program - CALC_SVO.C _ 

Purpose - perform servo lookup table calculations toTMjV II 
Author - LT MJ. CLOUTIER 


#mclude <math.h> 
#include <stdio.h> 


FILE *outfp; 


void calc_servo_val(angle) 
double angle; 

{ 

double voltage; 


angle += 45.0; /* normalizes angle to 45 */ 
if (angle < 15.0) 

voltage = (angle*0.086333)+0.27; 
else if (angle < 30.0) 

voltage = ((angle - 15.0)*0.08466) + 1.565; 
else if (angle < 45.0) 

voltage = ((angle - 30.0)*0.068) + 2.835; 
else if (angle < 60.0) 

voltage = ((angle - 45.0)*0.055) + 3.855; 
else if (angle <= 75.0) 

voltage = ((angle - 60.0)*0.04666) + 4.68; 
else if (angle <= 90.0) 

voltage = ((angle - 75.0)*0.04266) + 5.38; 
else { 

printf("Invalid angle: %4.2ftfi",angle - 45.0); 
exit(-l); 

} 

fprintf(outfp,'V%d,\n",(int)( 102.4 * voltage)); 

} r calc_servo_ang */ 

main (argc,argv) 
int argc; 
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char *argv[]; 

{ 

double ang; 


if ((outfp = fopen("seivo.dat","w")) == 0){ 
printf("Unable to open ’svo.datAn"); 
exit(-l); 

} 

fprintf(outfp,"int svo_ang[91] = {\n"); 
for (ang = -45.0; ang <= 45.0; ang += 1.0){ 
calc_servo_val(ang); 

} 

fprintf(outfp,"} ;\n''); 
fclose(outfp); 

} r CALC_SVO */ 

/* 

* S.DAT - servo lookup table for AUV II 

* Author - LT M.J. CLOUTIER 
V 

int svo ang[91] = { 

27,36,45,54,63,71,80,89,98,107,116,124,133,142,151,160,168, 

177,186,194,203,212,220,229,238,246,255,264,272,281,290,297, 

304,311,318,325,332,339,346,352,359,366,373,380,387,394,400, 

406,411,417,422,428,434,439,445,451,456,462,467,473,479,484, 

488,493,498,503,507,512,517,522,527,531,536,541,546,550,555, 

559,564,568,572,577,581,585,590,594,598,603,607,612,616 

}; 


r 

* Program — TEST_SVO.C 

* Purpose — servo test program for AUV II 

* Author —- LT M.J. CLOUTIER 
V 

#include <stdio.h> 

#include <math.h> 

#include "s.dat" 

#define DAC_ADDR OxFFFOOOOO 
#define DAC_LSB_OFFSET 0x2 
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#define BOW 


0 


unsigned char *dac_a = DAC_ADDR; 

* dac(s,ch) - writes signal ’s’ to adal dac channel ’ch’ 

* (allowable channels 0-3) 

void dac(s,ch) 
int s,ch; 

{ 

ch = ch < < 2; I* offset for G-96 addressing */ 

dac_a[ch] = s >> 2; I* write upper 8 bits to MSBV 

dac_a[ch -I- DAC_LSB_OFFSET] = s < < 6; r write lower 2 bits B3,B2 */ 
return; 

} /* dac */ 


void send_servo(surface,angle) 

/* 

* This function sends the desired ANGLE to the specified control SURFACE 

* The angle is first normalized to (-45 to 45), then correction is applied 

* for the non-linearity in the servo control module 
*/ 

int surface; 
double angle; 

{ 

double voltage; 

if ((angle < -45.0) 11 (angle > 45.0)){ 

printf("Angle out of range: %4.2f\n",angle); 

} else { 

dac(svo_ang[(int)(angle + 45.0)],surface); 

} 

} /* send_servo *! 

main () 

{ 

double ang; 
long j; 

ang = 0.0; 

while ((ang <= 45.0) && (ang >= -45.0)){ 

printf("Enter desired angle from -45 to -1-45 degrees (999 to quit)\n"); 
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scanf("%lf',«&ang); 

send_servo(BOW,ang); 

} 

send_servo(BOW,0.0); 

} r TEST_SVO.C */ 
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APPENDIX D. MAIN MOTOR CONTROL CODE 


r 

* Program - CALC^MN.C 

* Purpose - perform main motor lookup table calculations for AUV II 

* Author - LT M.J. CLOUTIER 
*/ 

#include <math.h> 

#include <stdio.h> 

FILE *outfp; 

void calc_rpm(rpm) 
double rpm; 

{ 

double voltage; 
if (rpm < -700){ 

printf("Invalid RPM: %4.2fai",rpm); 
exit(.l); 

} 

else if (rpm < -600) 

voltage = ((2.73 - 2.01)/100*(rpm-K700)) -i- 2.01; 
else if (rpm < -500) 

voltage = ((3.25 - 2.73)/100*(rpm->-600)) 2.73; 

else if (rpm < -400) 

voltage = ((3.74 - 3.25)/100*(rpm-1-500)) + 3.25; 
else if (rpm < -300) 

voltage = ((4.14 - 3.74)/100*(rpm-j-400)) -f 3.74; 
else if (rpm < -200) 

voltage = ((4.45 - 4.14)/100*(rpm-l-300)) + 4.14; 
else if (rpm < -100) 

voltage = ((4.67 - 4.45)/100*(rpm-l-200)) + 4.45; 
else if (rpm < 0) 

voltage = ((5.0 - 4.67)/100*(rpm-l-100)) -f 4.67; 
else if (rpm < 100) 

voltage = ((5.32 - 5.0)/100*rpm) + 5.0; 
else if (rpm < 200) 

voltage = ((5.56 - 5.32)/100*(rpm-100)) + 5.32; 
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else if (rpm < 300) 

voltage = ((5.90 - 5.56)/100*(rpm-200)) + 5.56; 
else if (rpm < 400) 

voltage = ((6.39 - 5.90)/100*(rpm-300)) + 5.90; 
else if (rpm < 500) 

voltage = ((6.90 - 6.39)/100*(rpm-400)) + 6.39; 
else if (rpm < 600) 

voltage = ((7.45 - 6.90)/100*(rpm-500)) + 6.90; 
else if (rpm < = 700) 

voltage = ((8.28 - 7.45)/100*(rpm-600)) + 7.45; 
else { 

printf("Invalid rpm: %4.2f\n",rpm); 
exit(-l); 

} 

fprintf(outfp,'V%d,\n",(int)( 102.4 * voltage)); 

} /* calc_rpm *! 

main (argc,argv) 
int argc; 
char *argv[]; 

{ 

double rpm; 

if ((outfp = fopen("main_mtr.dat'V'w")) == 0){ 
printf("Unable to open ’main_mtr.dat’\n"); 
exit(-l); 

} 

fprintf(outfp,"int main_mtr[141] = {\n"); 
for (rpm = -700.0; rpm <= 700.0; rpm += 10.0){ 
calc_rpm(rpm); 

} 

fprintf(outfp,"};\n"); 

fclose(outfp); 

} r CALC_MN.C 

/* 

* S.DAT - servo lookup table for AUV II 

* Author - LT M.J. CLOUTIER 

V 

int main_mtr[141] = { 

205,213,220,227,235,242,250,257,264,272,279,284,290,295, 
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300,306,311,316,322,327,332,337,342,347,352,357,362,367, 
372,377,382,387,391,395,399,403,407,411,415,419,423,427, 
430,433,436,439,442,446 ' 9,452,455,457,460,462,464,466, 
469,471,473,475,478,48-, .^,488,491,495,498,501,505,508, 
512,515,518,521,525,528,531,534,538,541,544,547,549,552, 
554,557,559,561,564,566,569,572,576,579,583,586,590,593, 
597,600,604,609,614,619,624,629,6-4,639,644,649,654,659, 
664,670,675,680,685,690,696,701,706,712,717,723,729,734, 
740,745,751,757,762,771,779,788,796,805,813,822,830,839, 
847, 
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Program — TEST_MN.C 

Purpose — main motor test program for AUV II 

Author —- LT M.J. CLOUTIER 


/* 

♦ 

V 

#include <math.h> 

#inc]ude <stdio.h> 

#include "m.dat" 

/* address of ADAl (could also use DAC2B) */ 

#define DAC_ADDR OxFFFOOOOO 
#define DAC_LSB_OFFSET 0x2 
I* PORT_MAIN is channel 1 of ADAl */ 

#define PORT_MAIN 1 

unsigned char *dac_a = DAC ADDR; 

* dac(s,ch) " writes signal ’s’ to adal dac channel ’ch’ 

* (allowable channels 0-3) 

1^^i|^i|^i|^^^:^i^^t:^i**’¥**^^***■****^l^*^^^H^*^^^^*^^*********^‘************’>^ j 

void dac(s,ch) 
int s,ch; 

{ 

ch = ch < < 2; /* offset for G-96 bus addressing *! 

dac_a[ch] = s > > 2; I* write upper 8 bits to MSB *! 

dac_a[ch -i- DAC_LSB_OFFSET] = s < < 6; I* write lower 2 bits B3,B2 */ 
return; 

} /* dac */ 

void main_rpm(motor,rpm) 
int motor; 
double rpm; 

{ 

rpm += 700.0; /* to normalize to 0-1400 RPM */ 

dac(main_mtr[(int)(rpm/10.0)],motor); 

}!* main_rpm */ 

main () 

{ 

double rpm; /* RPM value to send to MAIN_RPM */ 


85 





rpm = 0.0; 

while ((rpm <= 700.0) && (rpm >= -700.0)){ 

prmtf("Enter desired RPM value from -700 to +700 RPM (999 to quit)\n"); 

scanf("%lf',&rpm); 

main_rpm(PORT_MAIN,rpm); 

} 

mam_rpm(PORT_MAIN,0.0); 

} /* TEST_MN.C V 


86 




APPENDIX E. GLOSSARY 


Waypoint - 

a quadruple (x,y,z,v) consisting of position at the intersection of grid points in 
a ^obal rectangular three-dimensional grid coordinate system (x,y,z) and linear 
velocity (v) in the x direction of a vehicle centered coordinate system. 

Posture - 

a sextuple (x,y,z,fl,(^,v) consisting of position (x,y,z) in a three-dimensional 
rectangular coordinate system (not constrained to grid intersections), velocity 
as defined above, heading (0) in the x-y plane, and pitch angle (<f>) in the x-z 
plane. 

Reference Posture - 

Reference postures are generated by STEP_SPIRAL. They are postures which 
are located on the current cubic spiral somewhere between the last waypoint 
and the current waypoint. The velocity in a reference posture is taken directly 
from the current waypoint. 

Desired Posture - 

A Reference Posture which has had navigational compensation applied to 
position, angles and velocity based on actual vehicle position and velocity 
parameters. Desired Postures are generated by the Tracking Controller and 
are provided as input to the Autopilot. 

Current Posture - 

The current vehicle posture consists of current position and velocity as 
determined by Navigation. 
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APPENDIX F. OFF-LINE GUIDANCE SOURCE CODE 


/* 

* WAYPT.DAT - the required input file for FIND_CS.C 

* These are the 27 possible combinations of grid points for AUV II 
*/ 

0 0 1 1 0 2-1 1 
0 0 1 1 0 2-1 2 
00 1 1 02-1 3 

00111203 

00111213 

00111223 

00112233 

00112232 

00112231 

00102112 

00102122 

00102132 

00102031 
00102030 
0 0 1 0 2 0 3 -1 

0 0 1 0 2-1 3 -2 
0 0 1 0 2-1 2-2 
0 0 1 0 2-1 1 -2 

0 0 1 -1 2-23 -1 
0 0 1 -1 2-23 -2 
0 0 1 -1 2-23 -3 

0 0 1 -1 1 -22-3 
0 0 1-11-21-3 
0 0 1 -1 1 -20-3 


0 0 1-10-2-1-3 
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0 0 1 - 10 - 2 - 1-2 
0 0 1 - 10 - 2 - 1-1 


* 


♦ 

* 

* 

« 

* 

♦ 

* 

4c 

♦ 

* 

4c 

4c 

4i 

4 c 

** 


find_cs.c - This program is used to determine the appropriate 

cubic spiral path for the NPS AUV local path-planner. 

It is a modification of the cubic spiral calculation 
program developed by Yutaka Kanayama for Yamabico-11. 

LT M.J. CLOUTIER 
12/18/89 

Inputs - file consisting of sets of four waypoints 

(expressed as doubles) consisting of x and y position 

Outputs - file consisting of one or more cubic spiral values 
for A and 1 


#include <stdio.h> 

#include <math.h> 

#include "cst.h" 

/* required for cot function (doesn’t exist in TurboC) */ 

#define TURBOC 0 

* MACROS 

#define SQR(x) ( (x) * (x) ) 

#defme CUBE(x) ((x) * (x) * (x)) 

#define EUCL_DIST(xl,yl^,y2) (sqrt(((xl) - (x2)) * ((xl) - (x2)) -I- ((yl) - (y2)) * 

((yi) - (y2)))) 

#define DIST(xl,ylpc2,y2) (fabs((xl) - (x2)) + fabs((yl) - (y2))) 

#define r2d(r) ((r) * RAD) 

!************’¥***************** **********i^*^ti*titim****m****ti 

* DEFINED CONSTANTS 
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#define TRUE_ 1 
#defineFALSE_ !TRUE_ 

#define SUCCESS 1 
#define FAILURE ISUCCESS 

y4c*4c*4c***4c*4c*«*********4i4i*4c*«4c4i4i«4c*«4i4t**4c**4i******4c***4i4c4c4c4c 

* NUMERICAL CONSTANTS 


#define PI_ 

#define NPI 
#define DPI 
#define HPI 
#define RAD 
#define ZERO_RAD 
#define NAMESIZE 


3.14159265358979323846 
-3.14159265358979323846 
6.28318530717958647692 
1.57079632679489661923 
57.29577951308232087684 
0.01 
20 


r4c4i*4i4c4c*4i4>4<4c4>4<4<4>4c4‘4i4>4>4c4>4c4c4c*4<il<4c4c:ti4i4c%^i|i:|ii|i^i|iitic|cc«c4i4cittitictc4tctc4c4c4c4>*4cc*4‘ 

* VARIABLES 

4>c»4i4c*4<4citc4ci|c4c4i4i4i4i4c4t4c4t4c4c4ci|c4i%4c:tc!tcitciti4iiti:tiitc4c4c4c4c4i4c4c4c4c4c4c4c4>c»4>4c*4<4>4c4i4c4c4> 


typedef struct posture { 

char postureJd[NAMESIZE]; 

double x; 

double y; 

double theta; 

double k; 

} 

POSTURE; 

FILE *infp, *outfp; 

^4i4c4c4i4[4c4c4c4c4cc^i*4c4c:^4i4i4i4ii^4i4i4citc4c:ti4i:tic|c4ci|c4c4c4ii^4i4i4iitiiti4ic4i:tic^:ti4i;)i;tciti4i:tc4c4c4c4c4c4c4c 

* zero rad - TRUE if data is < = ZERO_RAD 

int zero_rad(data) 
double data; 

{ 

return ((fabs(data) <= ZERO_RAD) ? TRUE_ : FALSEJ; 
} /* zero rad */ 
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* cot - returns the trigonometric cotangent 

* (TurboC doesn’t provide cot!!!) 

#ifdef TURBOC 
double cot(arg) 
double arg; 

{ 

double tmp; 
if (zero_rad(arg)) { 

fprintf(stderr,"cot: bad argument %7.3f\n",arg); 
exit(-l); 

} else { 

return (1.0 / tan(arg)); 

} 

} 

#endif 
/* cot */ 

* create_posture - assigns x,y,theta to posture 

m**^ittitLitillliilititi^********iti****^*i****^^^^**^^* ****************** *^ 

i 

POSTURE *create_posture(x,y,theta,p) 
double X, y; 
double theta; 

POSTURE *p; 

{ 

p->x = x; 

p->y = y; 

p-> theta = theta; 
retum(p); 

} /* create_posture */ 

littili3lt*iti**^HLmHLHiHiitt:^^ii^Ht^iitLHi:^i^iHi*il/i^Hciti7lc7tiilfilftti:$f^********************** 

* pnorm - positive normalization (0 < a < 2PI) 

****************:^lt*********m************ ******************* 

double pnorm(a) 
double a; 

{ 

while (a >= DPI) 
a -= DPI; 
while (a < 0) 
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a += DPI; 
retum(a); 

} /* pnorm */ 

* norm - normalizes angle between -PI and PI 

double norm(a) 
double a; 

{ 

while (a > PI_) 
a -= DPI; 
while (a <= NPI) 
a += DPI; 
return(a); 

} /* norm */ 

y]|t4i4c4c4t4(4t4c4c4t4c4c4c4c)|E«4c4c3^4c*4c4t4c4c4c4c4c4c4(4c«4t%4E4>4t4c4c)|e4c*4t4c4c4c*4c*4c4c4c4(4t4t*4c«%4t4c4(4:4c*9K 
« 

4c 
4t 
4c 
« 

4c 
4c 
4c 
4« 

4c 
4> 

4c 
4c 

4t4i4i4i4i4(4i4i4e4^4c4i4(4c4c4(4(4(4ii^4(4(4(4i4(4c4c4c4(]^#4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4(4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c 


LOOKUP_CURVE - returns normalized distance for value looked 
up in cst.h 


\ 

cst[al] I-\ 

I |\ 

dist I-1™\ 

I I |\ 

cst[a2] I- 

I_l_l_L\ 

al tmp a2 


double lookup_curve(alfa) 
double alfa; 

{ 

int alfal, alfa2; 
double dist, tmp; 


tmp = r2d(alfa); 
alfal = (int) tmp; 
alfa2 = alfal + 1; 
tmp = tmp - alfal; 


/* because cst.h is in degrees */ 

/* get cst value below tmp */ 

/* get cst value above tmp */ 

I* set tmp to delta btwn alfal and tmp */ 
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if (alfal <= 281) { 

/* interpolate dist based on proximity of alfa to integer values */ 
dist = cst[alfa2] * tmp + cst[alfal] * (1 - tmp); 

} else { I* zero crossing undefined -> set to 1000 */ 
dist = 1000; 

} 

return (dist); 


} /* lookup_curve */ 

* COSTF - used to calculate best split point when two CS are 

* required. Returns the local cost of an ALFA,DIST pair 

* where cost is ratio of angle squared to length cubed 

* (from Smooth Path Planning Paper) 

* 

* Minimum cost occurs if ANGLE is ZERO. Minimum cost 

* for a given angle occurs when length is large (small 

* curvature) 

* len 1 dist 

* - = - => len =- 

* dist D(alfa) D(alfa) 

* 

double costf(alfa,dist) 
double alfa, dist; 

{ 

return(SQR(alfa) * CUBE(lookup_curve(fabs(alfa))/dist)); 

} /* costf */ 


4c 

* COST - returns the cost of a given trajectory, where cost 

* is defined to be sum of the costs of two separate 

* DIST,ALPHA pairs. 

* 

* xc,yc are center of circle which is locus of mid-points 

* symmetric to both (xl,yl) and (x2,y2). 

* r is radius of circle, g is angle to r 
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X (xc,yc) 


* (xl,yl) I /|\ 

* X -> I X (x2,y2) 

* \ I / 

* \ I r / 

* \ I / 

* distl \ I / dist2 

* \ I / 

* \/ 

* 


double cost(xl,yl,thetalpc2,y2,theta2^c,yc,r,g) 
double xl,yl,thetalpc2,y2,theta2p(c,yc,r,g; 

i 

I 

double x,y,d,distl,dist2,alphal,alpha2; 

/* find center of circle */ 

X = xc + r * cos(g); 
y = yc + r * sin(g); 
distl = EUCL_DIST(xl,ylpc,y); 
dist2 = EUCL_DIST(x2,y2,x,y); 

r 2.0 * forces symmetry (SPP Paper) *! 
alphal = 2.0 * norm(atan2(y - yl, x - xl) - thetal); 
alpha2 = 2.0 * norm(atan2(y2 - y, x2 - x) - theta2); 

/* Total cost is cost of both CS */ 
return(costf(alphal,distl) + costf(alpha2,dist2)); 

} /* cost */ 

* SPLIT - splits a set of postures that require two cubic spirals 

* into two separate postures with the proper mid-point 


POSTURE *split(p_, q_, Q) 

POSTURE *p_, *q_, *0; 

{ 

double CO, xc, yc, r, xm, ym, theta_mid, gl, g2, g, w; 
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double xl, yl, thetal, x2, yl, theta2, alpha, alphal, betal, etal, eta2, wl; 
double costg, costgl,costg2; 

xl = p_->x; 

yl = p_.>y; 

thetal = p_-> theta; 
x2 = c|_->x; 
y2 = q_->y; 
theta2 = q_-> theta; 

/* determine if symmetric/parallel */ 
alpha = norm(theta2 - thetal); 
alphal = fabs(alpha); 

/* if the postures are parallel then split at midpoint of line between */ 
if (alphal < ZERO_RAD) { 
xm = (xl + x2) / 2.0; 
ym = (yl + y2) / 2.0; 

} else { /* find the best midpoint based on minimum cost *! 

/* first find the center of locus of valid midpoints */ 

CO = cot(alpha / 2.0); 
xc = (xl + x2 + CO * (yl - y2)) / 2.0; 
yc = (yl + y2 + CO * (x2 - xl)) / 2.0; 
r = EUCL_DIST(xl,ylpcc,yc); 

/* now pick proper values for quadrant */ 
if (alpha > 0.0) { 

gl = atan2(yl - yc, xl - xc); 
g2 = atan2(y2 - yc, x2 - xc); 
etal = thetal - HPI; 
eta2 = theta2 - HPI; 

} else { 

gl = atan2(y2 - yc, x2 - xc); 
g2 = atan2(yl - yc, xl - xc); 
etal = theta2 + HPI; 
eta2 = thetal + HPI; 

} 

/* g is direction to r from (xc,yc), w is increment for search */ 
g = (gl + g2) / 2.0; 
w = alphal / 2.0; 
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/* pick proper chunk of curve to explore (to minimize search time */ 
if (((wl = pnorm(etal - gl)) < alphal) && (2.0 * wl < alphal)) { 
g = etal; 
w = wl; 

} 

if (((wl = pnorm(g2 - eta2)) < alphal) && (2.0 * wl < alphal)) { 
g = eta2; 
w = wl; 

} 

/* iterate g until get minimum cost */ 
costg= cost(xl,yl,thetal^2,y2,theta2pcc,yc,r,g); 
while (w > ZERO_RAD) { 
w = w / 2.0; 

costgl= cost(xl,yl,thetal^2,y2,theta2pcc,yc,r,g+w); 
if(costgl < costg) { 
g = g + w; 
costg = costg 1; 

} else { 

costg! = cost(xl,yl,thetal,x2,y2,theta2^c,yc,r,g-w); 
if (costg2 < costg) { 
g = g • w; 
costg = costg2; 

> 

} 

}; 

/* now calculate midpoint for split */ 
xm = xc + r * cos(g); 
ym = yc + r * sin(g); 


/* create new posture for midpoint of split */ 
betal= atan2(ym - yl, xm - xl); 
theta_mid = betal + norm(betal - thetal); 
create_posture(xm,ym,theta_mid,Q); 
retum(Q); 

} r split V 


* SOLVEl - gives simple turn solution using cubic spiral 
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solve l(p, q) 

POSTURE *p, *q; 

{ 

double alpha, len; 

/* these calculations are same as above (COST) from SPP Paper */ 

alpha = 2 * norm(atan2(q->y - p->y, q->x - p->x) - p->theta); 

len = EUCL_DIST(q->x, q->y, p->x, p->y) / loolmp_curve(fabs(alpha)); 

/* write into instruction buffer */ 
fprintf(outfp,"%16.12f,%16.12f',len,6*alpha/(CUBE(len))); 

} /* solve 1 */ 

* SOLVE - returns a single posture (or posture pair) based on input 

* pairs of (x,y,theta) 


int solve(pp, qq) 

POSTURE *pp, -^qq; 

{ 

POSTURE p,q,midpst; 
double beta; 

p = *pp; 
q = *qq; 

/* check if valid input */ 

if ((zero_rad(p.y - q.y)) && (zero_rad(p.x - q.x))) { 

fprintf(stderr,"Bad waypts (%d,%d) (%d,%d)\n",p.x,p.y, q.x,q.y); 
exit(-l); 

} 

/* find angle between postures */ 
beta = atan2((q.y - p.y), (q.x - p.x)); 

/* if symmetric use one CS */ 
fprintf(outfp,'^t{"); 

if (fabs(norm(q.theta - beta) - norm(beta - p.theta)) < ZERO_RAD) { 
solve l(pp, qq); 

fprintf(outfp,",% 16.12f,% 16.12f’,0.0,0.0); 

} else { /* split and use two cubic spirals*/ 
split(pp, qq, &midpst); 
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solvel(pp, &midpst); 

Q)rintf(outfp,'V'); 

solvel(&inidpst, qq); 

} 

fprintf(outfp,"},\n"); 

return (SUCCKS); 

} /* solve */ 


main(argc,argv) 
int argc; 
char *argv[]; 

{ 

int scan_result; 

double xl,yl^2,y2,x3,y3pc4,y4; 

double theta 1, thetai; 

POSTURE p,q; 

/* open input and output files */ 
if ((infp = fopen(argv[l],"r")) == 0) { 

printf("Unable to open %s for input\n",argv[l]); 
exit(-l); 

} 

if ((outfp = fopen(argv[2],"w")) == 0) { 

printf("Unable to open %s for output\n",argv[2]); 
exit(-l); 

} 

/* get each set of waypts */ 

while ((scan_result = fscanf(infp,"%lf %lf %lf %lf %lf %lf %lf %lf', 
&xl,&yl,&x2,&y2,&x3,«&y3,&x4,&y4)) != EOF) { 

/* Determine the angles */ 
thetal = atan2((y3 - yl),(x3 - xl)); 
theta2 = atan2((y4 - y2),(x4 - x2)); 

/* create the postures */ 
p.x = x2; 
p.y = y2; 

p. theta = thetal; 

q. x = x3; 
q.y = y3; 
q.theta = theta2; 
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/* and calculate ’A’ and ’1’ */ 
solve(&p,&q); 

} 

} /* main */ 

/* find_cs.c */ 
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APPENDIX G. REAL-TIME GUIDANCE SOURCE CODE 


« 

* Ipp.h - Header file for NPS AUV Local Path-Planner 

* (adapted from mml.h for Yamabico-11 by 

* Yutaka Kanayama) 

* 

* LT M.J. CLOUTIER 

* 12/19/89 

* 

* Revl - 1/24/90 

* 


♦ MACROS 


#define SQR(x) ( (x) * (x) ) 

#define CUBE(x) ((x) * (x) * (x)) 

#define EUCL DIST(xl,ylpc2,y2) (sqrt(((xl) - (x2)) * ((xl) - (x2)) -t- ((yl) - (y2)) * 

((yi) - (y2)))) ■ 

#define DIST(xl,ylpc2,y2) (fabs((xl) - (x2)) + fabs((yl) - (y2))) 

#define SQRT(xc) (sqrt(1.0 + (xc)*(xc))) 

#define SIGN(x) (((x) < 0.0) ? 0x01 : ((x) > 0.0) ? 0x02 : 0x00) 

#define r2d(r) ((r) * RAD) 

#define d2r(d) ((d) / RAD) 

* DEFINED CONSTANTS 

♦♦*♦**♦♦♦***♦♦*♦♦♦♦*♦♦**♦♦♦**♦*♦♦*******♦**********♦♦♦*♦♦♦/ 


#define TRUE_ 1 
#define FALSE_ ITRUE 
#define ON 1 

#define OFF !ON 

#define YES 1 

#define NO !YES 

#define SUCCESS 1 
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#define FAILURE ISUCCESS 

• 

/* THESE MAY BE CHANGED AS REQUIRED V 
#define GRID SZ 180 /* 70in ~ = 180cm V 

#defme MAX3 tEPS 200 
#define CLOSE_ENOUGH 0.5 
#define INTRVL 0.0001 

#define RPM 2_VEL 0.04921 I* in/sec*sec/min*min/rev*cm/in (350rpm=4fps)*/ 

#define K X " 10 

#defineKlY 0.0064 

#define K HDG 0.16 

#define m1n_X_ERROR 1 

#define CSTA 361 /* size of cubic spiral table */ 

#define BUFSIZE 1024 /* buffer size for getstr *! 

#define MAXLINE 40 /* max string length */ 

#define NAMESIZE 20 /* name size for posture *! 

* NUMERICAL CONSTANTS 


#defme PI_ 3.14159265358979323846 

#define DPI 6.28318530717958647692 /* PI*2 */ 

#define RAD 57.29577951308232087684 I* 180/PI V 

#defme HPI 1.57079632679489661923 /* PI/2 */ 

#define QPI 0.78539816339744830%2 /* PI/4 (was PI4) */ 

#define ZERO_RAD 0.01 

#define NO_SPIRAL 0 

#define SINGLE 1 

#define DOUBLEl 2 

#define DOUBLE2 3 

#define N 0x01 

#denne Z_ 0x00 

#define P 0x02 

#define NN 0x05 

#define NZ 0x04 

#define NP 0x06 

#define ZN 0x01 

#define ZZ 0x00 

#define ZP 0x02 
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#define PN 

0x09 

#define PZ 

0x08 

#define PP 

OxOa 

#define NEG 

-1 

#define ZERO 

0 

#define POS 

1 

#define LET 

7 

#define CENTER 0 

#define RGHT 

' -7 

#define L 

0x01 

#define C 

0x00 

#define R 

0x02 

#define TJ.T 

0x15 

#define LLC 

0x14 

#define LLR 

0x16 

#define LCL 

0x11 

#define LCC 

0x10 

#define LCR 

0x12 

#define LRL 

0x19 

#define LRC 

0x18 

#define LRR 

Oxla 

#define CLL 

0x05 

#define CLC 

0x04 

#derine CLR 

0x06 

#define CCL 

0x01 

#define CCC 

0x00 

#define CCR 

0x02 

#define CRL 

0x09 

#define CRC 

0x08 

#define CRR 

OxOa 

#define RLL 

0x25 

#define RLC 

0x24 

#define RLR 

0x26 

#define RCL 

0x21 

#define RCC 

0x20 

#define RCR 

0x22 

#define RRL 

0x29 

#define RRC 

0x28 

#define RRR 

0x2a 





r ipp.h */ 


* curves.def - xy-plane curve descriptors for ’guidances’ 

struct DESCRIPTOR{ 
double 11,A1,12,A2; 

}; 


struct 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 

{ 


DESCRIPTOR desc[43] = { 
1 . 000000000000 , 0 . 000000000000 }, 
0.426861119501, -20.677652279190, 
0.426861119501, 20.677652279190, 


0.000000000000}, 
1.265295220167}, 
-6.072725411590, 
5.272056359189, 
0.000000000000}, 
1.450450440178, -1.265295220167}, 
0.832934089063, -5.272056359189, 
6.072725411590, 


0.000000000000, 

1.450450440178, 

0.475286674856, 

0.832934089063, 

0.000000000000, 


0.475286674856, 

0.000000000000, 

0.000000000000, 

0.000000000000, 

0.000000000000, 

0.000000000000, 

0.600965473691, 

1.054318417319, 

0.527159208659, 

0.000000000000, 

1.082989700434, 

1.652500089655, 

0.902131678928, 

0.000000000000, 

0.600005018599, 

1.414213562373, 

0.628250853818, 

0.000000000000, 

0.000000000000, 


0.000000000000}, 

0.000000000000}, 

0.000000000000}, 

0.000000000000}, 

0.000000000000}, 

20.227029553329, 

4.747378858590}, 

37.979030868724, 

0.000000000000}, 

5.743097253612, 

2.088558538488}, 

10.078999384402, 

0.000000000000}, 

5.166871437198, 

0.000000000000}, 

10.838140077661, 

0.000000000000}, 

0.000000000000}, 


0.600965473691, 20.227029553329}, 
0.600965473691, -20.227029553329}, 


1.082989700434, 5.743097253612}, 
0.600005018599, -5.166871437198}, 

0.600005018599, 5.166871437198}, 
1.082989700434, -5.743097253612}, 

0.426861119501, -20.677652279190}, 
0.527159208659, -37.979030868724}, 
0.475286674856, -6.072725411590}, 
0.628250853818, -10.838140077661}, 
0.832934089063, -5.272056359189}, 
0.902131678928, -10.078999384402}, 
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{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0.600965473691,-20.227029553329, 0.426861119501, 20.677652279190}, 
{ 0.527159208659,-37.979030868724, 0.527159208659, 37.979030868724}, 
{ 1.054318417319, -4.747378858590}, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0.600005018599, -5.166871437198, 0.832934089063, 5.272056359189}, 
{ 0.628250853818,-10.838140077661, 0.902131678928, 10.078999384402}, 
{ 1.414213562373, -0.000000000000}, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 1.082989700434, -5.743097253612, 0.475286674856, 6.072725411590}, 
{ 0.902131678928,-10.078999384402, 0.628250853818, 10.838140077661}, 
{ 1.652500089655, -2.088558538488} 

}; 


* 

* guidance.c - This program is the local path-planner for the NPS 

* AUV. It reads in waypoints from an input file, 

* determines the appropriate curve or curve pair for 

* the given waypoints and then calls the stepper. It 

* writes to two separate files as indicated below 

* 

* LT M.J. CLOUTIER 

♦ 

* Inputs - file (argv[l]) of waypoints consisting of x,y position 

* Outputs - file (argv[2]) of input descriptors for stepper 

* - file (argv[3]) of stepper output postures 

* 

* Rev4 - 1/11/90 - changed get_init_data to return (x0,y0,card_hdg) 

* Rev3 - 1/10/90 - changed dir_chg to operate on sign of norm(hdg) 

* - changed get_descriptor to tbl lookup (vs switch) 

* - default cardinal_hdg is now determined from first 

* two way-pts 

* - eliminated atan2 using tbl lookup 

* - shifted output coord system [abs(HPI-theta)] 

* Rev2 - 1/09/90 - incorporated GRID_SZ 

* Revl - 1/07/90 - initialized ref_theta, init_hdg 

* Orig - 12/21/89 
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♦ 


#include <stdio.h> 

#include <math.h> 

#include "Ipp.h" 

#include "curves.def' 

double act_x, act_y, act_z, act_v, act_hdg; 

#define act_omega 0.0 

extern struct DESCRIPTOR; I* (from ’curves.def) */ 

r exported GLOBALS V 

double des_x, des_y, des z, des_v, des_hdg; /* values to AUTOPILOT */ 

I* globals */ 

int x_last,y_last,z_last,vjast;/* holds next goal values from MP */ 
int x_next,y_next,z_next; /* current goal position from MP */ 
int v_next; /* next desired velocity from MP */ 

int v_curr; /* current velocity */ 

int triple; /* the current turn combo (eg. LLL) */ 

double cardinal_hdg; /* vehicle cardinal heading (N,S,E,W) */ 

struct DESCRIPTOR cd; /* descriptor for current spiral */ 

double dist_trvl; /♦ distance traveled on current spiral */ 

double st_x, st_y, st_hdg; /* values from stepper */ 

FILE *infp, *outfp; /* input path, output posture files */ 

int eofjnfp; I* eof pointer for input */ 

double x_start, y_start, z_start; /* initial position *! 

int curve_type; /* (NO_SPIRAL,SINGLE,DOUBLEl,DOUBLE2) V 

* MISCELLANEOUS MATH FUNCTIONS 


He 

* ATAN2 - returns table lookup of angle to replace 

* atan2 

* 


r TABLE LOOKUP VALUES *! 


105 




double xy_ang[5][5] = { 
-2.356194490192344840, 
-2.034443935795702710, 
-1.570796326794896560, 
-1.107148717794090410, 
-0.785398163397448279, 
-2.677945044588986970, 
-2.356194490192344840, 
-1.570796326794896560, 
-0.785398163397448279, 
-0.463647609000806094, 
3.141592653589793120, 
3.141592653589793120, 
0.000000000000000000, 
0.000000000000000000, 
0.000000000000000000, 
2.677945044588986970, 
2.356194490192344840, 
1.5707%326794896560, 
0.785398163397448279, 
0.463647609000806094, 
2.356194490192344840, 
2.034443935795702710, 
1.570796326794896560, 
1.107148717794090410, 
0.785398163397448279}; 


double ATAN2(y2,ylpc2,xl) 
int y2,ylpc2pcl; 

{ 

return xy_ang[(y2-yl)/GRID_SZ+2][(x2-xl)/GRID SZ-l-2]; 
} r ATAN2 V 

♦ 

* norm - normalizes angle between -PI and PI 

* 

double norm(a) 
double a; 

{ 

while ((a > PI ) 11 (a < = -PI_)) 

{ 
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if (a > PIJ 
a = a - DPI; 
else 

a = a + DPI; 

} 

return (a); 

} /* norm *! 

y4c 4t 4 e 4c 4c 41 4c 3|i 4c ^ * 4^ 4^ 4c ]|c 4c 4c 4c ^ 4c 4^ 

4c 

* dir_chg - returns the relative direction chg (L,C or R) 

* based on input waypoints. dir_chg uses the 

* global "cardinal_hdg" to determine rel chg 

4c 

4c4c*4t4c4c4t4c4t4c4c4c4<4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4t4c4‘4c1'1c4c4c4c4c4c4c4c4c 

int dir_chg(xl,yl,x2,y2) 
int xl,ylp(2,y2; 

{ 

double heading; 
int rel_dir; 

heading = ATAN2(y2,ylpc2,xl); 
heading -= cardinal_hdg; 
rel_dir = (int)(10.0*norm(heading)); 
switch (rel_dir){ 

case LFT : cardinal_hdg += HPI; 
return L; 

case CENTER : return C; 
case RGHT : cardinal hdg -= HPI; 
return R; 

default : fprintf(stderr,"dir_chg: Bad result in dir_chg\n"); 
exit(-l); 

} 

} /* dir_chg */ 

y4E4c4c4c4c4c4c4(^4t4c4c*4c«4(4c4c4c4>4c*4c4(4c4t4c4c4c4(4(4c4c4c4c4c4c4c4c*4c4c4c4c4(4c4c4c4c4c4c4t4c4c4c4c4c4c 

4c 

* process_next_waypt - reads next waypt from infp and returns 

* the next curve descriptor 

4i4t*4t4c4c4c4c4c4t4c#4t^4c4ci^4c]^«4c4c4(4c4t4c:f4c4(4c*4c4c4c4c^^4c4c4c4c4c4c4c4c4c4c*4c4c4c4c*4c4e4c4c4c 

void process_next_waypt() 

{ 
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int mp_x, mp_y, mp_z, mp_v; r values from MISSION PLANNER */ 


if ((eof_infp = fscanf(infp,"%d %d %d %d",&mp_x,&mp_ 3 ^,&mp_z,&mp_v)) 
EOF){ 

triple = (triple & OxOf) < < 2 | dir_chg(xjast,y_last,mp_x,mp_y); 
cd = desc[triple]; 

v_cuiT = v_next; I* this is target velocity */ 

x_next = x_last; /* this is current goal */ 
y_next = y.Jast; 
z_next = zjast; 
v_next = v_last; 

xjast = mp_x; /* this is next goal */ 
yjast = mp_y; 
z_last = nip_z; 
v_last = mp_v; 

/* for now the desired depth is the same as reference */ 
des_z = (double)z_next; 

} 

} /* process_next_waypt */ 

♦ 

* next_pos - calculates next reference values for 

* X, y and theta 

* 

void next_pos(Al) 
double Al; 

{ 

double theta, del_theta, step, t step; 
double kappa; 

r $$$$$$$$ DEBUGGING VALUES $$$$$$$$ V 
act_x = st_x; 
act_y = st_y; 
act_hdg = st_hdg; 

^^^^^^^^^^^ 4 ^ / 

step = des_v * RPM_2_VEL * INTRVL; 
dist_trvl += step; 
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kappa = A * dist_trvl * (1 - dist_trvl); 
del_theta = kappa * step; 
st_hdg += del_theta; 
theta = st_hdg - del_theta/2.0; 
if (fabs(deT_theta) == 0.0) { 
t_step = step; 

} else { 

t_step = step * (sin(del_theta / 2.0)/(del_theta / 2.0)); 

} 

st_x += cos(theta) * t_step; 
st_y += sin(theta) * t_step; 

} /* next_pos */ 

♦ 

* print_stepper_out - prints x,y and theta to step outfp 

♦ 

void print_stepper_out() 

{ 

fprintf(outfp,"%d %d %8.4f\n", 

st_x, st_y, norm(fabs(HPI - st_hdg))); 

} /* print_stepper_out */ 

^Jt‘**********^**Jlt**:tci^******ilc}tc*:^^i>r’..=t.*************>ttK*Hc******Hi* 

♦ 

* print_desired_out - prints x.y and theta to step_outfp 

♦ 

void print_desired_out() 

{ 

r 

fprintf(outfp,"%8.4f %8.4f %8.4f %8.4f %8.4f\n", 
des_x + x_start, des_y + y_start, des_z + z_start, 
des_v ,norm(fabs(HPI - des_hdg))); 

V 

fprintf(outfp,"%8.4f %8.4f\n", 

des_x + x_start, des_y + y_start); 

} /* print_desired_out */ 

* 

* step_spiral - breaks spiral into discrete postures, 
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* prints to output file with break between 

* curves 

4c 

void step_spiral() 

{ 

int step_ct; 


step_ct = 0; 

while(step_ct < MAX_STEPS){ 
switch(curve_type) { 

case NO_SPIPAL: dist_trvl = 0.0; 

/* get spiral type */ 
if (cd.l2 == 0.0){ 

curve_type = SINGLE; 

} else ■{ 

curvetype = DOUBLE 1; 

> 


r step to next_pos *! 

for (;((dist_trvl <= cd.ll) && (step_ct < MAX_STEPS)); 

step_ct+ + ){ 

next_pos(cd.Al,cd.ll); 

} 

break; 

case SINGLE; 

case DOUBLEl; for(;((dist_trvl <= cd.ll) && (step_ct < MAX_STEPS)); 
step_ct+ + ){ 

next_pos(cd.Al,cd.ll); 

} 


step_ct+ + ){ 


/* finished this spiral, reset dist_trvled, do next */ 
if (dist_trvl >= cd.ll){ 
disttrvl = 0.0; 

/* if second part exists = > start along it */ 
if (cd.l2 != 0.0){ 

curve_type = DOUBLE2; 

for (;((dist_trvl < = cd.l2)&&(step_ct < MAX_STEPS)): 
next_pos(cd.A2,cd.l2); 

} 


} else { /* go to next way_pt */ 
curve_type = NO_SPIRAL; 

} 


no 





break; 

case DOUBLE2: for (;((dist_trvl < = cd.l2) && (step ct < 

MAX_STEPS)); step_ct++){ 

next_pos(cd.A2,cd.l2); 

} 

if (dist_trvl >= cd.l2){ 
dist_trvl = 0.0; 
curvetype = NO_SPIRAL; 

} 

break; 

default: fprintf(stderr,"step_spiral: Bad input value\n"); 

} 

if (curve_type == NO_SPIRAL){ 
process_next_waypt (); 

} 

} 

printf(" Ipp (x,y) (%4.3f,%4.3f) MP (x,y) (%d,%d)\n", 

st_x*(double)GRID_SZ+x_start,st_y*(double)GRID_SZ+y_start,x_next,y_next); 

print_desired_out(); 

} /* step_spiral */ 


* calc_des_val() - finds error values and desired values 

* to feed to AUTOPILOT 

* 

void calc_des_val() 

{ 

double err_x, err_y, err_hdg; 

err_x = (((st_x - act_x)*cos(act_hdg)) + ((st_y - act_y)*sin(act_hdg)))/2.57; 
err_y = (((act_x - st_x)*sin(act_hdg)) + ((st_y - act_y)*cos(act_hdg)))/2.57; 
err_hdg = norni(st_hdg - act_hdg); 
des_x = (st_x + err_x) * (double)GRID_SZ; 
des_y = (st_y + err_y) * (double )GRID_SZ; 
des_v = (K_X * err_x) + ((double)v_curr * cos(err_hdg)); 
des_hdg += INTRVL*(act_omega + (v_curr * (K_Y * err_y + K_HDG * 
err_hdg))); 

/* if not too far behind, then get next st_ values else use old ones */ 
if (err_x <= MIN_X_ERROR){ 
step_spiral(); 


} 






} I* calc_des_val */ 

« 

* open_files() - open input & output files 

♦ 

void open_files() 

{ 

if ((infp = fopen("path.in","r")) == 0) { 
printf("Unable to open ’path-in^n''); 
exit(-l); 

} 

if ((outfp = fopen("path.out","w")) == 0) { 
printf("Unable to open ’path.out’\n"); 
exit(-l); 

} 

} /* open_files */ 

y4c3|c3|t4e4c***4c3^4i**^3((4c]|c4(:tc«4i4c]tc3ti3(c)(c4c4:}^4c3^4c3(c4c4c3|c3|c«3tc4c]tc]tc:tt]tc9^4c#9|c](E4c4c9(c3t::tc**^:tc 

* 

* getjnit_values - get initial Ipp values 

♦ 

void get init_values() 

{ 

int dxl,dyl,dx2,dy2: 
int dirl,dir2,dir3; 
int x_curr, y_curr, z_curr; 
int x_past, yj3ast, z_past; 

/* initialize variables */ 

x_past = y_past = 0; 

x_curr = y_curr = z_curr = v_curr = 0; 

x_next = y_next = z_next = v_next = 0; 

x_last = yjast = z_last = vjast = 0; 

r this is the first time through, so read four waypoints */ 
if ((eofjnfp = fscanf(infp,"%d %d %d %d %d %d %d %d %d %d %d %d", 
&x_curr,&y_curr,&z_curr,«fev_curr, 

&x_next,&y_next,&z_next,&v_next, 

4fex_last,«S:y_last,«&zJast,&v_last)) != EOF) { 
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/* get initial data *! 
dxl = SIGN(x_next - x_curr); 
dyl = SIGN(y_next - y_curr); 
dx2 = SIGN(x_last - x_next); 
dy2 = SIGN(y_last - y_next); 
x_start = x_curr; 
y_start = y_curr; 
z_start = z_curr; 

/* this switch determines a valid position PRIOR to start */ 
switch((dxl << 2) I dyl){ 

case PZ : cardinal hdg = ZERO; 

x_past = x_curr - GRID SZ; 

y_past = y_curr; 

break; 

case PP : switch ((dx2 << 2) | dy2){ 

case PN : cardinal_hdg = HPI; 
x_past = xcurr; 
y_past = y curr - GRID SZ; 
break; 

default : cardinal_hdg = ZERO; 

xjDast = x_curr - GRID_SZ; 
y_past = y curr; 

} 

break; 

case PN : switch ((dx2 << 2) | dy2){ 

case PP : cardinal_hdg = -HPI; 
xjDast = x_curr; 
yjDast = y_curr -I- GRID SZ; 
break; 

default : cardinal_hdg = ZERO; 

x_past = x_curr - GRID SZ; 
y_past = y_curr; 

} 

break; 

case ZP : cardinal_hdg = HPI; 
xj3ast = x_curr; 
y_past = y curr - GRID SZ; 
break; 

case ZN : cardinal_hdg = -HPI; 
x_past = x_curr; 
yjDast = y_curr -I- GRID_SZ; 
break; 
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case NZ : cardinal_hdg = PI_; 

xjjast = x_curr + GRIDSZ; 

y_past = y_cuiT; 

break; 

case NP : switch ((dx2 << 2) j dy2){ 

case NN : cardina!_hdg = HPI; 
x_past = x_cuit; 
y_past = y curr - GRID_SZ; 
break; 

default : cardinal_hdg = PI_; 

x_past = xcurr + GRIDSZ; 
y_past = y_curr; 

} 

break; 

case NN : switch ((dx2 << 2) | dy2){ 

case NP : cardinal_hdg = -HPI; 
x_past = x_curr; 
y_past = y_curr -f GRID SZ; 
break; 

default : cardinal_hdg = PI_; 

x_past = x_curr -I- GRID_SZ; 
yjDast = y_curr; 

} 

break; 

default : fprintf(stderr,"get_init_hdg: Bad input\n"); 
exit(-l); 

} 

/* now get triple */ 

dirl = dir_chg(x_past,y_past,x_curr,y_curr); 
dir2 = dir_chg(x_curr,y_curr,x_next,y_next); 
dir3 = dir_chg(x_next,y_next,x_last,y_last); 
triple = (dirl << 4) | (dir2 <<2)1 dir3; 

/* and ’current’ vehicle heading */ 

st_hdg = ATAN2(y_next,yj3ast,x_next,x_past); 

/* for now the desired depth is MP depth */ 
des_hdg = st_hdg; 
des_z = (double) z_next; 
des_v = (double) v_curr; 

} else { 

fprintf(stderr,"lpp_step main: not enough waypoints for path\n"); 
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exit(-l); 

} 

/* read curve information */ 
cd = desc[triple]; 

/* and get first desired values */ 
step_spiral(); 

} /* get_init_values *! 

main() 

{ 

int setparm = TRUEj 

while (eofjnfp != EOF){ 
if (setparm){ 
open_files(); 
get_init_values(); 
setparm = FAL5E_; 

} 

calc_des_val(); 

} 

close(infp); 

close(outfp); 

} 

I* guidance.c *! 
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APPENDIX H. THREE DIMENSIONAL SOURCE CODE 

I* D10_70.DAT V 

0.0 0.0 1.0 0.0 2.0 0.03.0 0.0 

0.0 0.0 1.0 0.0 2.0 0.03.0 0.142857142857 

0.0 0.0 1.0 0.0 2.0 0.03.0-0.142857142857 

0.0 0.0 1.0 0.0 2.0 0.1428571428573.0 0.142857142857 

0.0 0.0 1.0 0.0 2.0 0.1428571428573.0 0.285714285714 

0.0 0.0 1.0 0.0 2.0 0.1428571428573.0 0.0 

0.0 0.0 1.0 0.0 2.0-0.1428571428573.0-0.142857142857 

0.0 0.0 1.0 0.0 2.0 -0.1428571428573.0 0.0 

0.0 0.0 1.0 0.0 2.0 -0.1428571428573.0-0.285714285714 


0.0 0.0 1.0 0.1428571428572.0 0.1428571428573.0 0.142857142857 

0.0 0.0 1.0 0.1428571428572.0 0.1428571428573.0 0.285714285714 

0.0 0.0 1.0 0.1428571428572.0 0.1428571428573.0 0.0 

0.0 0.0 1.0 0.1428571428572.0 0.2857142857143.0 0.285714285714 

0.0 0.0 1.0 0.1428571428572.0 0.2857142857143.0 0.428571428571 

0.0 0.0 1.0 0.1428571428572.0 0.2857142857143.0 0.142857142857 

0.0 0.0 1.0 0.1428571428572.0 0.03.0 0.0 

0.0 0.0 1.0 0.1428571428572.0 0.03.0 0.142857142857 

0.0 0.0 1.0 0.1428571428572.0 0.03.0 -0.142857142857 


0.0 0.0 1.0 -0.1428571428572.0-0.1428571428573.0-0.142857142857 

0.0 0.0 1.0 -0.1428571428572.0-0.1428571428573.0 0.0 

0.0 0.0 1.0 -0.1428571428572.0-0.1428571428573.0 -0.285714285714 

0.0 0.0 1.0 -0.1428571428572.0 0.03.0 0.0 

0.0 0.0 1.0 -0.1428571428572.0 0.03.0 0.142857142857 

0.0 0.0 1.0 -0.1428571428572.0 0.03.0-0.142857142857 

0.0 0.0 1.0 -0.1428571428572.0-0.2857142857143.0 -0.285714285714 

0.0 0.0 1.0 -0.1428571428572.0-0.2857142857143.0 -0.142857142857 
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0.0 0.0 


1.0 -0.1428571428572.0 -0.2857142857143.0 -0.428571428571 


* 

* d_10-70.def - curve descriptors based on 10/70 

* 


struct DESCRIPTOR d_10(43] = { 

{ 1 . 000000000000 , 0 . 000000000000 , 0 . 000000000000 , 0 . 000000000000 }, 
{ 0.375115235242,-5.066081287242, 0.625593247974, 2.839641101518}, 
{ 0.375115235242, 5.066081287242, 0.625593247974,-2.839641101518}, 
{ 0.505688790039, 6.550461901757, 0.505688790039,-6.550461901757}, 
{ 0.631932817268, 2.727297694553, 0.378921326300,-4.865469783722}, 
{ 0.506118022287, 8.183874149810, 0.507051887040,-11.420667239510}, 
{ 0.505688790039, -6.550461901757, 0.505688790039, 6.550461901757}, 
{ 0.506118022287, -8.183874149810, 0.507051887040, 11.420667239510}, 
{ 0.631932817268, -2.727297694553, 0.378921326300, 4.865469783722}, 
{ 0.625593247974,-2.839641101518, 0.375115235242, 5.066081287241}, 

{ 0.500619316543,-6.820142254254, 0.500619316543, 6.820142254254}, 

{ 1.001238633086,-0.852517781782, 0.000000000000, 0.000000000000}, 
{ 0.378921326300, 4.865469783722, 0.631932817268,-2.727297694553}, 

{ 1.010152544552, 0.000000000000, 0.000000000000, 0.000000000000}, 
{ 0.442414807789, 5.530421017324, 0.570239645211,-7.174182722803}, 
{ 0.507051887040,-11.420667239510, 0.506118022287, 8.183874149809}, 
{ 0.507556871107,-13.022683702248, 0.507556871107, 13.022683702248}, 
{ 0.570239645211,-7.174182722804, 0.442414807789, 5.530421017324}, 
{ 0.625593247974, 2.839641101518, 0.375115235242,-5.066081287241}, 
{ 1.001238633086, 0.852517781782, 0.000000000000, 0.000000000000}, 
{ 0.500619316543, 6.820142254254, 0.500619316543, -6.820142254254}, 
{ 0.507051887040, 11.420667239510, 0.506118022287, -8.183874149809}, 
{ 0.570239645211, 7.174182722804, 0.442414807789, -5.530421017324}, 

{ 0.507556871107, 13.022683702248, 0.507556871107,-13.022683702248}, 
{ 0.378921326300,-4.865469783722, 0.631932817268, 2.727297694553}, 
{ 0.442414807789,-5.530421017324, 0.570239645211, 7.174182722803}, 
{ 1.010152544552,-0.000000000000, 0.000000000000, 0.000000000000} 

}; 

I* D_7_70.dat */ 

0.0 0.0 1.0 0.0 2.0 0.03.0 0.0 

0.0 0.0 1.0 0.0 2.0 0.03.0 0.1 

0.0 0.0 1.0 0.0 2.0 0.03.0-0.1 
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0.0 

0.0 

1.0 

0.0 

2.0 0.13.0 0.1 

0.0 

0.0 

1.0 

0.0 

2.0 0.13.0 0.2 

0.0 

0.0 

1.0 

0.0 

2.0 0.13.0 0.0 

0.0 

0.0 

1.0 

0.0 

2.0 -0.13.0 -0.1 

0.0 

0.0 

1.0 

0.0 

2.0-0.13.0 0.0 

0.0 

0.0 

1.0 

0.0 

2.0 -0.13.0 -0.2 


0.0 

0.0 

1.0 

0.1 

2.0 

0.13.0 

0.1 

0.0 

0.0 

1.0 

0.1 

2.0 

0.13.0 

0.2 

0.0 

0.0 

1.0 

0.1 

2.0 

0.13.0 

0.0 

0.0 

0.0 

1.0 

0.1 

2.0 

0.23.0 

0.2 

0.0 

0.0 

1.0 

0.1 

2.0 

0.23.0 

0.3 

0.0 

0.0 

1.0 

0.1 

2.0 

0.23.0 

0.1 

0.0 

0.0 

1.0 

0.1 

2.0 

0.03.0 

o 

o 

0.0 

0.0 

1.0 

0.1 

2.0 

0.03.0 

0.1 

0.0 

0.0 

1.0 

0.1 

2.0 

0.03.0 

1 

P 

t—‘ 


0.0 

0.0 

1.0 -0.1 

2.0 -0.13.0 -0.1 

0.0 

0.0 

1.0 -0.1 

2.0-0.13.0 0.0 

0.0 

0.0 

1.0 -0.1 

2.0 -0.13.0 -0.2 

0.0 

0.0 

1.0 -0.1 

2.0 0.03.0 0.0 

0.0 

0.0 

1.0 -0.1 

2.0 0.03.0 0.1 

0.0 

0.0 

1.0 -0.1 

2.0 0.03.0 -0.1 

0.0 

0.0 

1.0 -0.1 

2.0 -0.23.0 -0.2 

0.0 

0.0 

1.0 -0.1 

2.0 -0.23.0 -0.1 

0.0 

0.0 

1.0 -0.1 

2.0 -0.23.0 -0.3 


♦ 


* d_7-70.def - curve descriptors for depth based on 7/70 grid 

♦ 




struct DESCRIPTOR d_7[43] = { 

{ 1 . 000000000000 , 0 . 000000000000 , 0 . 000000000000 , 0 . 000000000000 }, 
{ 0.375056870759,-3.550981208130, 0.625292414313, 1.992340756031}, 
{ 0.375056870759, 3.550981208130, 0.625292414313,-1.992340756031}, 
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{ 0.502797441596, 4.692973208712, 0.502797441596,-4.692973208712}, 

{ 0.628408287342, 1.953101936801, 0.376926970544,-3.481012896273}, 

{ 0.503006506962, 5.864754182372, 0.503461901514,-8.197729062929}, 

{ 0.502797441596,-4.692973208712, 0.502797441596, 4.692973208712}, 

{ 0.503006506962,-5.864754182372, 0.503461901514, 8.197729062929}, 

{ 0.628408287342,-1.953101936801, 0.376926970544, 3.481012896273}, 

{ 0.625292414313,-1.992340756031, 0.375056870759, 3.550981208131}, 

{ 0.500305048597,-4.787238600005, 0.500305048597, 4.787238600005}, 

{ 1.000610097194,-0.598404825001, 0.000000000000, 0.000000000000}, 

{ 0.376926970544, 3.481012896274, 0.628408287342,-1.953101936801}, 

{ 1.004987562112,-0.000000000000, 0.000000000000, 0.000000000000}, 

{ 0.439914558365, 3.951184960439, 0.566299789962,-5.145062579251}, 

{ 0.503461901514,-8.197729062929, 0.503006506962, 5.864754182371}, 

{ 0.503710502314,-9.358296801935, 0.503710502314, 9.358296801935}, 

{ 0.566299789962,-5.145062579251, 0.439914558365, 3.951184960440}, 

{ 0.625292414313, 1.992340756031, 0.375056870759, -3.550981208131}, 

{ 1.000610097194, 0.598404825001, 0.000000000000, 0.000000000000}, 

{ 0.500305048597, 4.787238600005, 0.500305048597,-4.787238600005}, 

{ 0.503461901514, 8.197729062929, 0.503006506962,-5.864754182371}, 

{ 0.566299789962. 5.145062579251, 0.439914558365, -3.951184960440}, 

{ 0.503710502314, 9.358296801935, 0.503710502314, -9.358296801935}, 

{ 0.376926i»/(,D44,-3.481012896274, 0.628408287342, 1.953101936801}, 

{ 0.43Sr-J ''58365,-3.951184960439, 0.566299789962, 5.145062579251}, 

{ 1.004987562112, 0.000000000000, 0.000000000000, 0.000000000000} 

}; 

* 

* xy.def - xy-plane curve descriptors for ’Ipp.c’ 

* 

struct DESCRIPTOR{ 
double 11,A1,12,A2; 

}; 

struct DESCRIPTOR desc[43] = { 

{ 1.000000000000, 0.000000000000}, 

{ 0.426861119501,-20.677652279190, 0.600965473691, 20.227029553329}, 
{ 0.426861119501, 20.677652279190, 0.600965473691,-20.227029553329}, 
{ 0.000000000000, 0.000000000000}, 

{ 1.450450440178, 1.265295220167}, 

{ 0.475286674856, -6.072725411590, 1.082989700434, 5.743097253612}. 

{ 0.832934089063, 5.272056359189, 0.600005018599, -5.166871437198}. 
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{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 1.450450440178, -1.265295220167}, 

{ 0.832934089063, -5.272056359189, 0.600005018599, 5.166871437198}, 

{ 0.475286674856, 6.072725411590, 1.082989700434, -5.743097253612}, 

{ 0.000000000000, 0.000000000000}, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0.600965473691, 20.227029553329, 0.426861119501,-20.677652279190}, 
{ 1.054318417319, 4.747378858590}, 

{ 0.527159208659, 37.979030868724, 0.527159208659,-37.979030868724}, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 1.082989700434, 5.743097253612, 0.475286674856, -6.072725411590}, 

{ 1.652500089655, 2.088558538488}, 

{ 0.902131678928, 10.078999384402, 0.628250853818,-10.838140077661}, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0.600005018599, 5.166871437198, 0.832934089063, -5.272056359189}, 

{ 1.414213562373, 0.000000000000}, 

{ 0.628250853818, 10.838140077661, 0.902131678928,-10.078999384402}, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0.600965473691,-20.227029553329, 0.426861119501, 20.677652279190}, 
{ 0.527159208659,-37.979030868724, 0.527159208659, 37.979030868724}, 
{ 1.054318417319, -4.747378858590}, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 0.600005018599, -5.166871437198, 0.832934089063, 5.272056359189}, 

{ 0.628250853818, -10.838140077661, 0.902131678928, 10.078999384402}, 
{ 1.414213562373, -0.000000000000}, 

{ 0 . 000000000000 , 0 . 000000000000 }, 

{ 1.082989700434, -5.743097253612, 0.475286674856, 6.072725411590}, 

{ 0.902131678928, -10.078999384402, 0.628250853818, 10.838140077661}, 
{ 1.652500089655, -2.088558538488} 

}; 

♦ 

* z.def - z-plane curve descriptors based on 10/70 grid 
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struct DESCRIPTOR z_desc[43] = { 

{ 1 . 000000000000 , 0 . 000000000000 , 0 . 000000000000 , 0 . 000000000000 }, 
{ 0.375115235242,-5.066081287242, 0.625593247974, 2.839641101518}, 
{ 0.375115235242, 5.066081287242, 0.625593247974,-2.839641101518}, 
{ 0.505688790039, 6.550461901757, 0.505688790039,-6.550461901757}, 
{ 0.631932817268, 2.727297694553, 0.378921326300,-4.865469783722}, 
{ 0.506118022287, 8.183874149810, 0.507051887040,-11.420667239510}, 
{ 0.505688790039,-6.550461901757, 0.505688790039, 6.550461901757}, 
{ 0.506118022287, -8.183874149810, 0.507051887040, 11.420667239510}, 
{ 0.631932817268,-2.727297694553, 0.378921326300, 4.865469783722}, 
{ 0.625593247974,-2.839641101518, 0.375115235242, 5.066081287241}, 
{ 0.500619316543,-6.820142254254, 0.500619316543, 6.820142254254}, 
{ 1.001238633086,-0.852517781782, 0.000000000000, 0.000000000000}, 
{ 0.378921326300, 4.865469783722, 0.631932817268,-2.727297694553}, 
{ 1.010152544552, 0.000000000000, 0.000000000000, 0.000000000000}, 
{ 0.442414807789, 5.530421017324, 0.570239645211, -7.174182722803}, 

{ 0.507051887040,-11.420667239510, 0.506118022287, 8.183874149809}, 
{ 0.507556871107,-13.022683702248, 0.507556871107, 13.022683702248}, 
{ 0.570239645211,-7.174182722804, 0.442414807789, 5.530421017324}, 
{ 0.625593247974, 2.839641101518, 0.375115235242,-5.066081287241}, 
{ 1.001238633086, 0.852517781782, 0.000000000000, 0.000000000000}, 
{ 0.500619316543, 6.820142254254, 0.500619316543,-6.820142254254}, 
{ 0.507051887040, 11.420667239510, 0.506118022287, -8.183874149809}, 
{ 0.570239645211, 7.174182722804, 0.442414807789, -5.530421017324}, 
{ 0.507556871107, 13.022683702248, 0.507556871107,-13.022683702248}, 
{ 0.378921326300,-4.865469783722, 0.631932817268, 2.727297694553}, 

{ 0.442414807789,-5.530421017324, 0.570239645211, 7.174182722803}, 
{ 1.010152544552,-0.000000000000, 0.000000000000, 0.000000000000} 

}; 

* guid xyz.c - This program is the three dimensional guidance 

* system for AUV 11. 

* 

* LT M.J. CLOUTIER 

* 04/17/90 

* 

* Inputs - file (argv[l]) of waypoints consisting of x,y position 

♦ 

* Outputs - file (argv[2]) of input descriptors for stepper 

* - file (argv[3]) of stepper output postures 

* 
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♦ 

♦ 

♦ 

* 

* 

♦ 

* 

♦ 

♦ 

♦ 

* 


Rev7 - 4/16/90 - added third dimension to Ipp 
Rev6 - 4/16/90 - attached another date to code 
Rev5 - 1/24/89 - modified code to operate with IRIS graphics 

routines 

Rev4 - 1/11/90 - changed get_init_data to return (x0,y0,card_hdg) 
Rev3 - 1/10/90 - changed dir_chg to operate on sign of norm(hdg) 

- changed get descriptor to tbl lookup (vs switch) 

- default cardinal_hdg is now determined from first 
two way-pts 

- eliminated atan2 using tbl lookup 

- shifted output coord system [abs(HPI-theta)] 

Rev2 - 1/09/90 - incorporated GRID_SZ 

Revl - 1/07/90 - initialized ref theta, init_hdg 
Orig - 12/21/89 




#include <stdio.h> 
#include <math.h> 
#include "Ipp.h" 
#include "xy.def' 
#include "z.def' 


double act_x, act_y, act_z, act_v, act_hdg; 

#define act_omega 0.0 

extern struct DESCRIPTOR; /* (from ’curves.def) */ 

r exported GLOBALS to IRIS/SYMBOLICS V 

double des_x, des_y, des_z, des_v, des_hdg, des_phi; /* values to AUTOPILOT */ 


/* globals */ 

int xjast,y_last,z_last,v_last; 

int x_next,y_next,z_next; 

int v_next; 

int v_curr; 

int triple; 

int z_triple; 

double cardinal_hdg; 

struct DESCRIPTOR cd; 

struct DESCRIPTOR zd; 

double d_trvl; 


/* holds next goal values from MP */ 

/* current goal position from MP */ 

/* next desired velocity from MP */ 

/* current velocity *! 
r the current turn combo (eg. LLL) */ 

/* vehicle cardinal heading (N,S,E,W) 
/* descriptor for current spiral 

/* distance traveled on current spiral */ 


V 
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double z_trvl; 

double st_x, st_y, st_z, st_hdg; I* values from stepper *! 
double z_hdg,st_xz; I* dummy for curve trvl calc */ 

FILE *infp, *outfp; /* input path, output posture files */ 

int eof_infp; /* eof pointer for input */ 

double x_start, y_start, z_start; /* initial position */ 
int curve_type; /* (NO_CRV,SNGL,DBLl,DBL2) V 

int z_ctype; 

* MISCELLANEOUS MATH FUNCTIONS 


♦ 

* ATAN2 - returns table lookup of angle to replace 

* atan2 


r TABLE LOOKUP VALUES */ 
double xy ang[5][5] = { 
-2.356194490192344840, 
-2.034443935795702710, 
-1.570796326794896560, 
-1.107148717794090410, 
-0.785398163397448279, 
-2.677945044588986970, 
-2.356194490192344840, 
-1.570796326794896560, 
-0.785398163397448279, 
-0.463647609000806094, 
3.141592653589793120, 
3.141592653589793120, 
0.000000000000000000, 
0.000000000000000000, 
0.000000000000000000, 
2.677945044588986970, 
2.356194490192344840, 
1.570796326794896560, 
0.785398163397448279, 
0.463647609000806094, 
2.356194490192344840, 
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2.034443935795702710, 

1.570796326794896560, 

1.107148717794090410, 

0.785398163397448279}; 

double ATAN2(y2,yl,x2,xl) 
int y2,ylpc2,xl; 

{ 

return xy_ang[(y2-yl)/GRID_SZ+2]((x2-xl)/GRID_SZ+2]; 

} /* ATAN2 V 

♦ 

* norm - normalizes angle between -PI and PI 

♦ 

double norm(a) 
double a; 

{ 

while (a > PI_) 
a -= DPI; 
while (a < -PI_) 
a -1-= DPI; 
return (a); 

} /* norm */ 

♦ 

* dir_chg - returns the relative direction chg (L,C or R) 

* based on input waypoints. dir_chg uses the 

* global "cardinal_hdg" to determine rel chg 

* 

I 

int dir_chg(xl,yl,x2,y2) 
int xl,yl,x2,y2; 

{ 

double heading; 
int rel_dir; 

heading = ATAN2(y2,yl,x2,xl); 
heading -= cardinal_hdg; 
rel_dir = (int)(10.0*norm(heading)); 
switch (rel_dir){ 
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case LFT : cardinal_hdg + = HPI; 
return L; 

case CENTER : return C; 
case RGHT : cardinal_hdg -= HPI; 
return R; 

default : fprintf(stderr,"dir_chg: Bad result in dir_chg\n"); 
exit(-l); 

} 

} /* dir_chg *! 

♦ 

* z_chg - returns the relative depth direction change (U,S,D) 

* uses normalized (Z GRID) depth diff for switch 

* 

« « 4c * i|c * 4c * 4( 4c ♦ « « 4c 4( ♦ * « * « 4c * 4c 4c ♦ ♦ * 4c ♦ ♦ * * 4^ 4c * 4c 4c 4c 4c « 4c 4c 4c ^ 

int z_chg(zl,z2) 
int zl,z2; 

{ 

switch ((zl - z2)/Z_GRID){ 
case -1: return U; 
case 0: return S; 
case 1; return D; 

default: fprintf(stderr,"z_chg; Bad resuU\n"); 
exit(-l); 

} 

} r z_chg */ 

^4c4c4c4c4c4c4(4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c4c«4c4c4c4c4c4c4c4c4c«4ciK4c4c4c4c4c**4c4c4c4c4c4(4c4c4c 

♦ 

* process_next_waypt • reads next waypt from infp and returns 

* the next curve descriptor 

4c 

4c4c4c4c4c4c4c4c4c4c4c3^4c4c4c4c4c**4c4c4c4c4c«4c4c4c«4c4c4t4c4c4i*4c4c4c4c4c4c4c4c4c4c4c4c*4c4c4c4c4c4c4c4c4c^ 

void process_next_waypt() 

{ 

int mp_x, mp_y, mp_z, mp_v; /* values from MISSION PLANNER */ 

if ((eofjnfp = fscanf(infp,"%d %d %d %d",&mp_x,&mp_y,&mp_z,&mp_v)) 
EOF){ 

triple = (triple & OxOf) < < 2 | dir_chg(xjast,yjast,mp_x,mp_y); 
z_triple = (z_triple & OxOf) < < 2 | z_chg(z_last, mp_z); 
cd = desc[triple]; 
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zd = z_desc[z_triple]; 

v_cuiT = v_next; /* this is target velocity */ 

x_next = x_last; /* this is current goal */ 
y_next = y_last; 
z_next = z_last; 
v_next = v_last; 

x_last = mp_x; /* this is next goal */ 
y_last = mp_y; 
z_last = mp_z; 
v_last = mp_v; 

} 

} /* process_next_waypt *! 

* next_pos - calculates next reference values for 

* X, y and theta 

void next_pos(del_s,A,l) 
double del s,A,l; 

{ 

double theta, del_theta, t_step; 
double kappa; 

/* $$$$$$$$ DEBUGGING VALUES $$$$$$$$ V 
act_x = st_x; 
act_y = st_y; 
act_hdg = st_hdg; 

d_trvl += del_s; 

kappa = A * d_trvl * (1 - d_trvl); 
del_theta = kappa * del s; 
st_hdg += del_theta; 
theta = st_hdg - del_theta/2.0; 
if (abs(del_theta) == 0.0) { 
t_step = del_s; 

} else { 

t_step = del_s * (sin(del_theta / 2.0)/(del_theta / 2.0)); 

} 

st_x += cos(theta) * t_step; 




st_y += sin(theta) * t_step; 

} !* next_pos */ 

* next_z - calculates next reference values for z 

void next_z(del_sz,A,l) 
double del sz,A,l; 

{ 

double phi, del_phi, t_sz; 
double kappa_z; 

DEBUGGING ************f 

act_z = st_z; 
z_trvl += del_sz; 

kappa_z = A * z_trvl * (1 - d_trvl); 
del_phi = kappa_z * del_sz; 
z_hdg += delj3hi; 
phi = z_hdg - del_phi/2.0; 
if (abs(del_phi) == 0.0){ 
t_sz = del_sz; 

} else { 

t_sz = del sz * (sin(del_phi / 2.0)/(deljphi / 2.0)); 

} 

st_xz += cos(phi) * t_sz; 
st_z += sin(phi) * t_sz; 

} /* next_pos */ 

* print_stepper_out - prints x,y and theta to step_outfp 

,lc******llt*i)t^im******************’l‘***************************^ 

void print_stepper_out() 

{ 

fprintf(outfp,"%d %d %8.4f\n", 

st_x, st_y, norm(fabs(HPI - st_hdg))); 

} 

* print_desired_out - prints x,y and theta to step_outfp 

m***********************************'**********************! 

void print_desired_out() 
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{ 

fprintf(outfp,"%8.4f %8.4f\n", des_x + x_start, des_y + y_start); 

} 

* step_spiral - breaks spiral into discrete postures, 

* prints to output file with break between 

* curves 

void step spiral() 

{ 

int xy_ct; 
int z_ct; 
double step; 
double z_step; 


xy_ct = 0; 
z_ct = 0; 

while(xy_ct < MAX_CT){ 
step = des_v * RPM_2_VEL * INTRVL; 
z_step = step * (cd.ll + cd.l2) / (zd.ll + zd.l2); 
switch(curve_type) { 

case NO_CRV: d_trvl = 0.0; 

/* get spiral type *l 
if (cd.l2 == 0.0){ 
curve_type = SNGL; 

} else { 

curve_type = DBLl; 

} 

/* step to next_pos */ 

for (;((d_trvl <= cd.ll) && (xy_ct < MAX_CT)); xy_ct+ + ){ 
next_pos(step,cd.A???d.l 1); 

} 

break; 
case SNGL: 

case DBLl: for (;((d_trvl <= cd.ll) && (xy_ct < MAX_CT)); xy_ct+ + ){ 
next_pos(step,cd. A l,cd.l 1); 

} 

/* finished this spiral, reset d_trvled, do next */ 
if (d_trvl >= cd.ll){ 
d_trvl = 0.0; 

/* if second part exists = > start along it */ 
if (cd.l2 != 0.0){ 
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curve_type = DBL2; 

for (;((d_trvl <= cd.l2) && (xy_ct < MAX_CT)); xy_ct+ + ){ 
next_pos(step,cd.A2,cd.l2); 

} 

} else { I* go to next way_pt */ 
curve_type = NO_CRV; 

} 

} 

break; 

case DBL2: for (;((d_trvl <= cd.l2) && (xy_ct < MAX CT)); xy_ct++){ 
next_pos(step,cd.A2,cd.I2); 

} 

if (d_trvl >= cd.l2){ 
d_trvl = 0.0; 
curve_type = NO_CRV; 

} 

break; 

default: fprintf(stderr,"step_spiral: Bad input value\n"); 

} 

switch (z_ctype){ 
case NO_CRV: z_trvl = 0.0; 

/* get spiral type */ 

if (curve_type != NO_CRV){ /* started a curve *! 
if (zd.l2 == 0.0) { 
z_ctype = SNGL; 

} else { 

z_ctype = DBLl; 

} 

r step to next_pos */ 

for (;((z_trvl <= zd.ll) && (z_ct < MAX_CT)); z_ct+ + ){ 
next_z(z_step,zd. A1 ,zd.l 1); 

/ 

} 

break; 
case SNGL: 

case DBLl: for (;((z_trvl <= zd.ll) && (z_ct < MAX_CT)); z_ct+ + ){ 
next z(z_step,zd.Al,zd.ll); 

} 

/* finished this spiral, reset z_trvled, do next */ 
if (z_trvl >= zd.ll){ 
z_trvl = 0.0; 

/* if second part exists = > start along it */ 
if (zd.l2 != 0.0){ 
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z_ctype = DBL2; 

for (;((z_trvl <= zd.l2) && (z_ct < MAX_CT)); z_ct++){ 
next_z(z step,zd.A2,zd.l2); 

} 

} else { /* go to next way_pt */ 
z_ctype = NO_CRV; 

} 

} 

break; 

case DBL2: for (;((z_trvl <= zd.l2) && (z_ct < MAX_CT));z_ct++){ 
next_z(z_step,zd.A2,zd.l2); 

} 

if (z_trvl >= zd.l2){ 
z_trvl = 0.0; 

z_ctype = NO_CRV; 

} 

break; 

default: fprintf(stderr,"step_spiral: Bad input value\n"); 

} 

if (curve_type = = NO_CRV){ 
process next_waypt(); 

} 

} 

printf(" Ipp (x,y,z) (%4.3f,%4.3f,%4.3f) MP (x,y,z) (%d,%d,%d)\n", 
st_x*(double)GRID_SZ+x_start,st_y*(double)GRID_SZ+y_start, 
st_z*(double)Z_GRID+z_startpc_next,y_next, z_next); 
print_desired_out(); 

} /* step_spiral */ 

* 

* calc_des_val() - finds error values and desired values 

* to feed to AUTOPILOT 

* 

void calc des val() 

{ 

double err_x, err_y, err_z, err_hdg; 

err_x = (((st_x - act_x)*cos(act_hdg)) + ((st_y • act_y)*sin(act_hdg)))/2.57; 
err_y = (((act_x - st_x)*sin(act_hdg)) + ((st_y - act_y)*cos(act_hdg)))/2.57; 
err_z = (((st_z - act_z)*cos(act_hdg)) + ((st_z - act_z)*sin(act_hdg)))/2.57; 
err_hdg = norm(st_hdg - act_hdg); 
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des_x = (st_x + err_x) * (double)GRID_SZ; 
des_y = (st_ 3 r + err_y) * (double )GRID_SZ; 
des z = (st_z + err_z) * (double)Z_GRlb; 
des_v = (K_X * err_x) + ((doubie)v_cuiT * cos(err_hdg)); 
des_hdg += INTRVL*(act_omega + (v_curr * (K_Y * err_y + K_HDG * 
err_hdg))); 

/*************TALK TO PROF KANAYAMA ********/ 

/* desjjhi += INTRVL*()V 

/* if not too far behind, then get next st values else use old ones */ 
if (err_x < = MIN_X_ERROR){ 
step_spiral(); 

} 

} /* calc_des_val */ 

♦ 

* open_files() - open input & output files 

void open_files() 

{ 

if ((infp = fopen("path.in","r")) = = 0) { 

printf("Unable to open ’path.in’\n"); 
exit(-l); 

} 

if ((outfp = fopen("path.out","w")) = = 0) { 
printf("Unable to open ’path.outVi"); 
exit(-l); 

} 

} /* open_files */ 

* 

* get_init_values - get initial Ipp values 

void get_init_values() 

{ 

int dxl,dyl,dx2,dy2; 
int dirl,dir2,dir3; 
int x curr, y_curr, z curr; 
int x_past, y_past, z_past; 
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/* initialize variables *! 

xjjast = y_past = 0; 

x_curr = y_curr = z_cuit = v_curr = 0; 

x_next = y_next = z_next = v_next = 0; 

x_last = y_last = zjast = vjast = 0; 

r this is the first time through, so read four waypoints *! 
if ((eof_infp = fscanf(in^,"%d %d %d %d %d %d %d %d %d %d %d %d", 
&x_curr,&y_curr,&z_curr,&v_curr, 

&x_next,&y_next,&z_next,&v_next, 

&x_last,&y_last,&zjast,&v_last)) != EOF) { 

/* get initial data */ 
dxl = SIGN(x_next - x_curr); 
dyl = SIGN(y_next - y_curr); 
dx2 = SIGN(x_last - x_next); 
dy2 = SIGN(y_last - y_next); 
x_start = x_curr; 
y_start = y_curr; 
z_start = z_curr; 

/* this switch determines a valid position PRIOR to start */ 
switch((dxl << 2) I dyl){ 
case PZ: cardinal^hdg = ZERO; 

x_past = x_curr - GRID_SZ; 

y_past = y_curr; 

break; 

case PP: switch ((dx2 << 2) | dy2){ 
case PN: cardinal_hdg = HPI; 
x_past = x_curr; 
y_past = y curr - GRID SZ; 
break; 

default: cardinal_hdg = ZERO; 

x_past = x_curr - GRID SZ; 
y_past = y_curr; 

} 

break; 

case PN: switch ((dx2 << 2) | dy2){ 
case PP: cardinal hdg = -HPI; 
x_past = x_curr; 
y_past = y_curr + GRID_SZ; 
break; 

default: cardinal_hdg = ZERO; 


132 






x_past = x_curr - GRID_SZ; 
y_past = y_curr; 

} 

break; 

case ZP: cardinal_hdg = HPI; 
x_past = x_cuit; 
y_past = y_curr - GRID SZ; 
break; 

case ZN: cardinal_hdg = -HPI; 
x_past = x_curr; 
y_past = y_curr + GRID_SZ; 
break; 

case NZ: cardinal_hdg = PI_; 

x_past = xcurr + GRID_SZ; 

yjDast = y_curr; 

break; 

case NP: switch ((dx2 << 2) | dy2){ 
case NN: cardinal_hdg = HPI; 
x_past = xcurr; 
y_past = y_curr - GRID SZ; 
break; 

default: cardinal_hdg = PIj 

x_past = x_curr + GRID_SZ; 
yjpast = y_curr; 

} 

break; 

case NN: switch ((dx2 << 2) | dy2){ 
case NP: cardinal_hdg = -HPI; 
x_past = x_curr; 
y_past = y_curr GRID SZ; 
break; 

default: cardinal_hdg = PIj 

x_past = x_curr + GRIDSZ; 
y_past = y_curr; 

} 

break; 

default: fprintf(stderr,"get_init_hdg: Bad input\n"); 
exit(-l); 

} 

/* now get triple */ 

dirl = dir_chg(x_past,y_pastpc_curr,y_curr); 

dir2 = dir_chg(x_curr,y_curr,x_next,y_next); 
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dir3 = dir_chg(x_next,y_nextpc_last,y_last); 
triple = (dirl << 4) | (dir2 <<2) | dir3; 

/* and ’current’ vehicle heading V 

st_hdg = ATAN2(y_next,y_pastpc_nextpc_past); 

/* for now the desired depth is MP depth */ 
deshdg = st_hdg; 
des_z = (double) z next; 
des_v = (double) v_curr; 

} else { 

fprintf(stderr,"lpp_step main: not enough waypoints for path\n"); 
exit(-l); 

} 

/* read curve information *! 
cd = desc[triple]; 

/* and get first desired values */ 
step_spiral(); 

} /* get_init_values */ 

main() 

{ 

int setparm = TRUEj 

while (eofjnfp != EOF){ 
if (setparm){ 
open_files(); 
get_init_values(); 
setparm = FALSEj 

} 

calc_des_val(); 

} 

close(infp); 

close(outfp); 

} 

/* guid_xyz.c */ 
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